Squashandroid-20111223

Change-Id: I2fcf46d1fc4b0cd4c61e5be3654c43b80db86015

120 files changed, 3771 insertions, 1989 deletions

diff --git a/Android.mk b/Android.mk
index 7dffa96c00d..640cc94f302 100644
--- a/Android.mk
+++ b/Android.mk
@@ -54,6 +54,20 @@ $(KERNEL_LIBPATH)/$(LOCAL_SRC_FILES): build-kernel
 
 include $(BUILD_PREBUILT)
 
+include $(CLEAR_VARS)
+
+KERNEL_LIBPATH := $(KERNEL_OUTPUT)/arch/arm/boot
+LOCAL_PATH := $(KERNEL_LIBPATH)
+LOCAL_SRC_FILES := zImage
+LOCAL_MODULE := $(LOCAL_SRC_FILES)
+LOCAL_MODULE_TAGS := optional
+LOCAL_MODULE_CLASS := EXECUTABLES
+LOCAL_MODULE_PATH := $(PRODUCT_OUT)
+
+$(KERNEL_LIBPATH)/$(LOCAL_SRC_FILES): build-kernel
+
+include $(BUILD_PREBUILT)
+
 # Configures, builds and installs the kernel. KERNEL_DEFCONFIG usually
 # comes from the BoardConfig.mk file, but can be overridden on the
 # command line or by an environment variable.
@@ -129,8 +143,5 @@ clean clobber : clean-kernel
 clean-kernel:
 	$(MAKE) $(PRIVATE_KERNEL_ARGS) clean
 
-$(INSTALLED_KERNEL_TARGET): $(PRODUCT_OUT)/uImage
-	ln -sf $(notdir $<) $@
-
 
 endif
diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 3b1c685136f..65d28aeb746 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1704,6 +1704,15 @@ config DEPRECATED_PARAM_STRUCT
 	  This was deprecated in 2001 and announced to live on for 5 years.
 	  Some old boot loaders still use this way.
 
+config ARM_FLUSH_CONSOLE_ON_RESTART
+	bool "Force flush the console on restart"
+	help
+	  If the console is locked while the system is rebooted, the messages
+	  in the temporary logbuffer would not have propogated to all the
+	  console drivers. This option forces the console lock to be
+	  released if it failed to be acquired, which will cause all the
+	  pending messages to be flushed.
+
 endmenu
 
 menu "Boot options"
diff --git a/arch/arm/configs/u8500_defconfig b/arch/arm/configs/u8500_defconfig
index f7843f524d4..f79a8183120 100755
--- a/arch/arm/configs/u8500_defconfig
+++ b/arch/arm/configs/u8500_defconfig
@@ -312,7 +312,6 @@ CONFIG_USB_G_ANDROID=y
 CONFIG_AB8500_USB=y
 CONFIG_MMC=y
 CONFIG_MMC_UNSAFE_RESUME=y
-CONFIG_MMC_CLKGATE=y
 CONFIG_MMC_PARANOID_SD_INIT=y
 # CONFIG_MMC_BLOCK_BOUNCE is not set
 CONFIG_MMC_ARMMMCI=y
@@ -406,7 +405,6 @@ CONFIG_DEBUG_INFO=y
 CONFIG_SYSCTL_SYSCALL_CHECK=y
 CONFIG_FUNCTION_TRACER=y
 CONFIG_DEBUG_USER=y
-CONFIG_DYNAMIC_DEBUG=y
 CONFIG_KEYS=y
 CONFIG_CRYPTO_MD5=m
 CONFIG_CRYPTO_SHA1=m
diff --git a/arch/arm/configs/u8500_snowball_android_defconfig b/arch/arm/configs/u8500_snowball_android_defconfig
index 1c406674762..aabc8e9f7b4 100644
--- a/arch/arm/configs/u8500_snowball_android_defconfig
+++ b/arch/arm/configs/u8500_snowball_android_defconfig
@@ -190,7 +190,6 @@ CONFIG_USB_G_ANDROID=y
 CONFIG_AB8500_USB=y
 CONFIG_MMC=y
 CONFIG_MMC_UNSAFE_RESUME=y
-CONFIG_MMC_CLKGATE=y
 CONFIG_MMC_PARANOID_SD_INIT=y
 # CONFIG_MMC_BLOCK_BOUNCE is not set
 CONFIG_MMC_ARMMMCI=y
diff --git a/arch/arm/configs/u9500_defconfig b/arch/arm/configs/u9500_defconfig
index d99b32e9f99..74817555488 100755
--- a/arch/arm/configs/u9500_defconfig
+++ b/arch/arm/configs/u9500_defconfig
@@ -315,7 +315,6 @@ CONFIG_USB_G_ANDROID=y
 CONFIG_AB8500_USB=y
 CONFIG_MMC=y
 CONFIG_MMC_UNSAFE_RESUME=y
-CONFIG_MMC_CLKGATE=y
 CONFIG_MMC_PARANOID_SD_INIT=y
 # CONFIG_MMC_BLOCK_BOUNCE is not set
 CONFIG_MMC_ARMMMCI=y
diff --git a/arch/arm/kernel/process.c b/arch/arm/kernel/process.c
index ccc587dbb95..23db0a90003 100644
--- a/arch/arm/kernel/process.c
+++ b/arch/arm/kernel/process.c
@@ -30,6 +30,7 @@
 #include <linux/uaccess.h>
 #include <linux/random.h>
 #include <linux/hw_breakpoint.h>
+#include <linux/console.h>
 
 #include <asm/cacheflush.h>
 #include <asm/processor.h>
@@ -90,8 +91,37 @@ static int __init hlt_setup(char *__unused)
 __setup("nohlt", nohlt_setup);
 __setup("hlt", hlt_setup);
 
+#ifdef CONFIG_ARM_FLUSH_CONSOLE_ON_RESTART
+void arm_machine_flush_console(void)
+{
+	printk("\n");
+	pr_emerg("Restarting %s\n", linux_banner);
+	if (console_trylock()) {
+		console_unlock();
+		return;
+	}
+
+	mdelay(50);
+
+	local_irq_disable();
+	if (!console_trylock())
+		pr_emerg("arm_restart: Console was locked! Busting\n");
+	else
+		pr_emerg("arm_restart: Console was locked!\n");
+	console_unlock();
+}
+#else
+void arm_machine_flush_console(void)
+{
+}
+#endif
+
 void arm_machine_restart(char mode, const char *cmd)
 {
+	/* Flush the console to make sure all the relevant messages make it
+	 * out to the console drivers */
+	arm_machine_flush_console();
+
 	/* Disable interrupts first */
 	local_irq_disable();
 	local_fiq_disable();
diff --git a/arch/arm/mach-ux500/Kconfig b/arch/arm/mach-ux500/Kconfig
index 27ff6c55bc1..0db5f1850b9 100644
--- a/arch/arm/mach-ux500/Kconfig
+++ b/arch/arm/mach-ux500/Kconfig
@@ -73,6 +73,32 @@ config KEYLAYOUT_LAYOUT2
 
 endchoice
 
+choice
+	prompt "DBx500 sched_clock"
+
+config DBX500_SCHED_CLOCK_PRCMU
+	bool "PRCMU Timer sched_clock"
+	depends on CLKSRC_DBX500_PRCMU
+	select CLKSRC_DBX500_PRCMU_SCHED_CLOCK
+	help
+	  Use the always on PRCMU Timer as sched_clock
+
+config DB5500_MTIMER_SCHED_CLOCK
+	bool "MTIMER sched_clock"
+	depends on CLKSRC_DB5500_MTIMER
+	select CLKSRC_DB5500_MTIMER_SCHED_CLOCK
+	help
+	  Use the always on MTIMER as sched_clock
+
+config DBX500_MTU_SCHED_CLOCK
+	bool "MTU sched_clock"
+	depends on HAS_MTU
+	select NOMADIK_MTU_SCHED_CLOCK
+	help
+	  Use the Multi Timer Unit as the sched_clock.
+
+endchoice
+
 config UX500_DEBUG_UART
 	int "Ux500 UART to use for low-level debug"
 	default 2
diff --git a/arch/arm/mach-ux500/board-mop500-mcde.c b/arch/arm/mach-ux500/board-mop500-mcde.c
index fd51f357faa..9d61e9ac3e4 100644
--- a/arch/arm/mach-ux500/board-mop500-mcde.c
+++ b/arch/arm/mach-ux500/board-mop500-mcde.c
@@ -313,7 +313,7 @@ static struct mcde_display_device av8100_hdmi = {
 	.port = &av8100_port2,
 	.chnl_id = MCDE_CHNL_B,
 	.fifo = MCDE_FIFO_B,
-	.default_pixel_format = MCDE_OVLYPIXFMT_RGB888,
+	.default_pixel_format = MCDE_OVLYPIXFMT_RGBA8888,
 	.native_x_res = 1280,
 	.native_y_res = 720,
 	.dev = {
diff --git a/arch/arm/mach-ux500/board-mop500-pins.c b/arch/arm/mach-ux500/board-mop500-pins.c
index 6241d490f2f..9788e45ec59 100644
--- a/arch/arm/mach-ux500/board-mop500-pins.c
+++ b/arch/arm/mach-ux500/board-mop500-pins.c
@@ -13,6 +13,7 @@
 #include <asm/mach-types.h>
 #include <plat/pincfg.h>
 #include <linux/gpio/nomadik.h>
+#include <linux/mfd/ab8500/gpio.h>
 
 #include <mach/hardware.h>
 #include <mach/suspend.h>
@@ -201,6 +202,12 @@ static pin_cfg_t snowball_pins[] = {
 	/* MMC0: MicroSD card */
 	GPIO21_MC0_DAT31DIR     | PIN_OUTPUT_HIGH,
 
+	/* Snowball buttons */
+	GPIO32_GPIO		| PIN_INPUT_PULLUP, /* User PB   */
+	GPIO151_GPIO		| PIN_INPUT_PULLUP, /* J1 pin 8  */
+	GPIO152_GPIO		| PIN_INPUT_PULLUP, /* J1 pin 9  */
+	GPIO162_GPIO		| PIN_INPUT_PULLUP, /* J1 pin 14 */
+
 	/* MMC2: LAN */
 	GPIO86_SM_ADQ0,
 	GPIO87_SM_ADQ1,
@@ -237,6 +244,7 @@ static pin_cfg_t snowball_pins[] = {
 
 	/* WLAN/GBF */
 	GPIO171_GPIO		| PIN_OUTPUT_HIGH,/* GBF_ENA */
+	GPIO161_GPIO		| PIN_OUTPUT_LOW, /* WLAN_PMU_EN */
 	GPIO215_GPIO		| PIN_OUTPUT_LOW,/* WLAN_ENA */
 	GPIO216_GPIO		| PIN_INPUT_PULLUP,/* WLAN_IRQ */
 };
@@ -246,23 +254,31 @@ static pin_cfg_t snowball_pins[] = {
  */
 
 static UX500_PINS(mop500_pins_i2c0,
-	GPIO147_I2C0_SCL,
-	GPIO148_I2C0_SDA,
+	GPIO147_I2C0_SCL |
+		PIN_SLPM_GPIO | PIN_SLPM_INPUT_NOPULL,
+	GPIO148_I2C0_SDA |
+		PIN_SLPM_GPIO | PIN_SLPM_INPUT_NOPULL,
 );
 
 static UX500_PINS(mop500_pins_i2c1,
-	GPIO16_I2C1_SCL,
-	GPIO17_I2C1_SDA,
+	GPIO16_I2C1_SCL |
+		PIN_SLPM_GPIO | PIN_SLPM_INPUT_NOPULL,
+	GPIO17_I2C1_SDA |
+		PIN_SLPM_GPIO | PIN_SLPM_INPUT_NOPULL,
 );
 
 static UX500_PINS(mop500_pins_i2c2,
-	GPIO10_I2C2_SDA,
-	GPIO11_I2C2_SCL,
+	GPIO10_I2C2_SDA |
+		PIN_SLPM_GPIO | PIN_SLPM_INPUT_NOPULL,
+	GPIO11_I2C2_SCL |
+		PIN_SLPM_GPIO | PIN_SLPM_INPUT_NOPULL,
 );
 
 static UX500_PINS(mop500_pins_i2c3,
-	GPIO229_I2C3_SDA,
-	GPIO230_I2C3_SCL,
+	GPIO229_I2C3_SDA |
+		PIN_SLPM_GPIO | PIN_SLPM_INPUT_NOPULL,
+	GPIO230_I2C3_SCL |
+		PIN_SLPM_GPIO | PIN_SLPM_INPUT_NOPULL,
 );
 
 static UX500_PINS(mop500_pins_mcde_tvout,
@@ -431,6 +447,7 @@ static pin_cfg_t mop500_pins_common_power_save_bank0[] = {
 
 	GPIO8_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO9_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	/* 10-11 - I2C2 */
 	GPIO10_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO11_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
@@ -439,8 +456,10 @@ static pin_cfg_t mop500_pins_common_power_save_bank0[] = {
 	GPIO14_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO15_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
+	/* 16-17 - I2C1 */
 	GPIO16_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO17_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
 	GPIO18_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO19_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
@@ -473,6 +492,7 @@ static pin_cfg_t mop500_pins_common_power_save_bank0_href60[] = {
 
 	GPIO8_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO9_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	/* 10-11 - I2C2 */
 	GPIO10_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO11_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
@@ -481,11 +501,12 @@ static pin_cfg_t mop500_pins_common_power_save_bank0_href60[] = {
 	GPIO14_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO15_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
+	/* 16-17 - I2C1 */
 	GPIO16_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO17_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
 	GPIO18_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO19_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
-
 	GPIO20_GPIO | PIN_SLPM_OUTPUT_HIGH | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO21_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO22_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
@@ -626,9 +647,11 @@ static pin_cfg_t mop500_pins_common_power_save_bank4[] = {
 	GPIO145_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
 	GPIO146_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
-	GPIO147_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
+	/* 147-148 - I2C0 */
+	GPIO147_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO148_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+
 	GPIO149_GPIO | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO150_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO151_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
@@ -813,6 +836,7 @@ static pin_cfg_t mop500_pins_common_power_save_bank7[] = {
 	GPIO227_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
 	GPIO228_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	/* 229-230 - I2C3 */
 	GPIO229_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO230_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 };
@@ -824,6 +848,7 @@ static pin_cfg_t mop500_pins_common_power_save_bank7_href60[] = {
 	GPIO227_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 
 	GPIO228_GPIO | PIN_SLPM_OUTPUT_LOW | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
+	/* 229-230 - I2C3 */
 	GPIO229_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 	GPIO230_GPIO | PIN_SLPM_DIR_INPUT | PIN_SLPM_WAKEUP_ENABLE | PIN_SLPM_PDIS_DISABLED,
 };
@@ -971,6 +996,15 @@ int pins_for_u9500(void)
 	return 0;
 }
 
+static UX500_PINS(mop500_offchip_gpio_cfg,
+	/*
+	 * Workaround for auto shutdown of 3.2MHz oscillator during
+	 * deep sleep. APESPICSn/GPIO37 must be floating on the board
+	 * to use this fix.
+	 */
+	AB8500_PIN_GPIO37 | PIN_OUTPUT_HIGH,
+);
+
 void __init mop500_pins_init(void)
 {
 	nmk_config_pins(mop500_pins_common,
@@ -985,16 +1019,6 @@ void __init mop500_pins_init(void)
 		ux500_pins_add(mop500_runtime_pins_old,
 			       ARRAY_SIZE(mop500_runtime_pins_old));
 
-	if (machine_is_hrefv60())
-		nmk_config_pins(mop500_pins_hrefv60,
-				ARRAY_SIZE(mop500_pins_hrefv60));
-	else if (machine_is_snowball())
-		nmk_config_pins(snowball_pins,
-				ARRAY_SIZE(snowball_pins));
-	else
-		nmk_config_pins(mop500_pins_default,
-				ARRAY_SIZE(mop500_pins_default));
-
 	switch (pinsfor) {
 	case PINS_FOR_U9500:
 		nmk_config_pins(u9500_pins, ARRAY_SIZE(u9500_pins));
@@ -1006,7 +1030,26 @@ void __init mop500_pins_init(void)
 		break;
 	}
 
+	if (machine_is_hrefv60())
+		nmk_config_pins(mop500_pins_hrefv60,
+				ARRAY_SIZE(mop500_pins_hrefv60));
+	else if (machine_is_snowball())
+		nmk_config_pins(snowball_pins,
+				ARRAY_SIZE(snowball_pins));
+	else
+		nmk_config_pins(mop500_pins_default,
+				ARRAY_SIZE(mop500_pins_default));
+
 	suspend_set_pins_force_fn(mop500_pins_suspend_force,
 				  mop500_pins_suspend_force_mux);
 }
 
+static int __init mop500_offchip_gpio_init(void)
+{
+	if (machine_is_hrefv60())
+                ux500_offchip_gpio_init(&mop500_offchip_gpio_cfg);
+
+	return 0;
+}
+/* Let gpio chip drivers initialize. */
+late_initcall(mop500_offchip_gpio_init);
diff --git a/arch/arm/mach-ux500/board-mop500-regulators.c b/arch/arm/mach-ux500/board-mop500-regulators.c
index 276d3d62512..62940ef78a5 100644
--- a/arch/arm/mach-ux500/board-mop500-regulators.c
+++ b/arch/arm/mach-ux500/board-mop500-regulators.c
@@ -13,6 +13,48 @@
 #include <linux/regulator/ab8500.h>
 #include "board-mop500-regulators.h"
 
+#ifdef CONFIG_REGULATOR_FIXED_VOLTAGE
+/*
+ * GPIO regulator controlled by the ab8500 GPIO16
+ */
+static struct regulator_consumer_supply gpio_wlan_vbat_consumers[] = {
+	/* for cg2900 chip */
+	REGULATOR_SUPPLY("vdd", "cg2900-uart.0"),
+	/* for cw1200 chip */
+	REGULATOR_SUPPLY("vdd", "cw1200_wlan"),
+};
+
+struct regulator_init_data gpio_wlan_vbat_regulator = {
+	.constraints = {
+		.name = "WLAN-VBAT",
+		.min_uV = 3600000,
+		.max_uV = 3600000,
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+	},
+	.num_consumer_supplies = ARRAY_SIZE(gpio_wlan_vbat_consumers),
+	.consumer_supplies = gpio_wlan_vbat_consumers,
+};
+
+/*
+ * GPIO regulator controlled by the ab8500 GPIO26
+ */
+static struct regulator_consumer_supply gpio_en_3v3_consumers[] = {
+	/* for LAN chip */
+	REGULATOR_SUPPLY("vdd33a", "smsc911x.0"),
+};
+
+struct regulator_init_data gpio_en_3v3_regulator = {
+	.constraints = {
+		.name = "EN-3V3",
+		.min_uV = 3300000,
+		.max_uV = 3300000,
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+	},
+	.num_consumer_supplies = ARRAY_SIZE(gpio_en_3v3_consumers),
+	.consumer_supplies = gpio_en_3v3_consumers,
+};
+#endif
+
 /*
  * TPS61052 regulator
  */
@@ -38,6 +80,10 @@ struct regulator_init_data tps61052_regulator = {
 };
 
 static struct regulator_consumer_supply ab8500_vaux1_consumers[] = {
+	/* lps001wp baromenter i2c dev name is 2-005c
+	 * maybe change that in the driver, like for lsm303dlh drivers
+	 */
+	REGULATOR_SUPPLY("vdd", "2-005c"),
 	/* Main display, u8500 R3 uib */
 	REGULATOR_SUPPLY("vddi", "mcde_disp_sony_acx424akp.0"),
 	/* Main display, u8500 uib and ST uib */
diff --git a/arch/arm/mach-ux500/board-mop500-regulators.h b/arch/arm/mach-ux500/board-mop500-regulators.h
index 0b94969604d..ed309081e14 100644
--- a/arch/arm/mach-ux500/board-mop500-regulators.h
+++ b/arch/arm/mach-ux500/board-mop500-regulators.h
@@ -16,5 +16,7 @@
 
 extern struct ab8500_regulator_platform_data ab8500_regulator_plat_data;
 extern struct regulator_init_data tps61052_regulator;
+extern struct regulator_init_data gpio_wlan_vbat_regulator;
+extern struct regulator_init_data gpio_en_3v3_regulator;
 
 #endif
diff --git a/arch/arm/mach-ux500/board-mop500-sdi.c b/arch/arm/mach-ux500/board-mop500-sdi.c
index 20be9e90eff..73de147832b 100644
--- a/arch/arm/mach-ux500/board-mop500-sdi.c
+++ b/arch/arm/mach-ux500/board-mop500-sdi.c
@@ -32,6 +32,11 @@ static int sdi0_vsel = -1;
 
 static int mop500_sdi0_ios_handler(struct device *dev, struct mmc_ios *ios)
 {
+	static int power_mode = -1;
+
+	if (power_mode == ios->power_mode)
+		return 0;
+
 	switch (ios->power_mode) {
 	case MMC_POWER_UP:
 	case MMC_POWER_ON:
@@ -53,6 +58,7 @@ static int mop500_sdi0_ios_handler(struct device *dev, struct mmc_ios *ios)
 		break;
 	}
 
+	power_mode = ios->power_mode;
 	return 0;
 }
 
diff --git a/arch/arm/mach-ux500/board-mop500-stuib.c b/arch/arm/mach-ux500/board-mop500-stuib.c
index 44a6d299c9f..a42f8081c70 100644
--- a/arch/arm/mach-ux500/board-mop500-stuib.c
+++ b/arch/arm/mach-ux500/board-mop500-stuib.c
@@ -55,11 +55,6 @@ static struct adp1653_platform_data __initdata adp1653_pdata_u8500_uib = {
 
 static struct i2c_board_info __initdata mop500_i2c2_devices[] = {
 	{
-		/* LSM303DLH Accelerometer */
-		I2C_BOARD_INFO("lsm303dlh_a", 0x18),
-		.platform_data = &lsm303dlh_pdata,
-	},
-	{
 		/* LSM303DLH Magnetometer */
 		I2C_BOARD_INFO("lsm303dlh_m", 0x1E),
 		.platform_data = &lsm303dlh_pdata,
@@ -81,6 +76,30 @@ static struct i2c_board_info __initdata mop500_i2c2_devices[] = {
 };
 
 /*
+ * Break this out due to the fact that this have changed address on Snowball
+ */
+static struct i2c_board_info __initdata mop500_2_i2c2_devices[] = {
+       {
+               /* LSM303DLH Accelerometer */
+               I2C_BOARD_INFO("lsm303dlh_a", 0x18),
+               .platform_data = &lsm303dlh_pdata,
+       },
+};
+
+/*
+ * This is needed due to the fact that the i2c address changed in Snowball V7 =<
+ * and there is no way of knowing if the HW is Snowball V7 or higher so we just
+ * have to try and fail.
+ */
+static struct i2c_board_info __initdata snowball_i2c2_devices[] = {
+       {
+               /* LSM303DLH Accelerometer */
+               I2C_BOARD_INFO("lsm303dlh_a", 0x19),
+               .platform_data = &lsm303dlh_pdata,
+       },
+};
+
+/*
  * ux500 keymaps
  *
  * Organized row-wise as on the UIB, starting at the top-left
@@ -172,6 +191,37 @@ static struct i2c_board_info __initdata mop500_i2c0_devices_stuib[] = {
 };
 
 /*
+ * Register/Add i2c sensors for Snowball
+ */
+void mop500_sensors_probe_add_lsm303dlh_a(void)
+{
+       static const int busnum = 2;
+       struct i2c_adapter *adap;
+       struct i2c_client *client;
+       static const unsigned short i2c_addr_list[] = {
+               0x18, 0x19, I2C_CLIENT_END };
+       struct i2c_board_info i2c_info = {
+                       /* LSM303DLH Accelerometer */
+                       I2C_BOARD_INFO("lsm303dlh_a", 0),
+                       .platform_data = &lsm303dlh_pdata,
+       };
+
+       adap = i2c_get_adapter(busnum);
+       if (!adap) {
+               /* We have no i2c adapter yet lets create it. */
+               pr_err(__FILE__ ": Could not get adapter %d\n", busnum);
+               return;
+       }
+       client = i2c_new_probed_device(adap, &i2c_info,
+                       i2c_addr_list, NULL);
+       if (!client)
+               pr_err(__FILE__ ": failed to register %s to i2c%d\n",
+                               i2c_info.type,
+                               busnum);
+       i2c_put_adapter(adap);
+}
+
+/*
  * BU21013 ROHM touchscreen interface on the STUIBs
  */
 
@@ -342,4 +392,25 @@ void __init mop500_stuib_init(void)
 	}
 	mop500_uib_i2c_add(2, mop500_i2c2_devices,
 			ARRAY_SIZE(mop500_i2c2_devices));
+
+
+    if (machine_is_snowball()) {
+        if (cpu_is_u8500v21()) {
+            /* This is ugly but we cant know what address
+             * to use */
+			printk("%s: **** Try to add Accelometer and cpu_is_u8500v21 \n",
+					__func__);
+            mop500_sensors_probe_add_lsm303dlh_a();
+        } else {/* Add the accelerometer with new addr */
+			printk("%s: **** Try to add Accelometer old way \n",
+					__func__);
+            mop500_uib_i2c_add(2, snowball_i2c2_devices,
+                           ARRAY_SIZE(snowball_i2c2_devices));
+		}
+    } else { /* none snowball have the old addr */
+	     printk("%s: **** Ops on snowball but not a snowball Accelometer\n",
+					__func__);
+         mop500_uib_i2c_add(2, mop500_2_i2c2_devices,
+                               ARRAY_SIZE(mop500_2_i2c2_devices));
+	}
 }
diff --git a/arch/arm/mach-ux500/board-mop500-u8500uib.c b/arch/arm/mach-ux500/board-mop500-u8500uib.c
index 9e1cafa8893..18d12083528 100644
--- a/arch/arm/mach-ux500/board-mop500-u8500uib.c
+++ b/arch/arm/mach-ux500/board-mop500-u8500uib.c
@@ -86,6 +86,7 @@ static struct synaptics_rmi4_platform_data rmi4_i2c_dev_platformdata = {
 	.irq_type       = (IRQF_TRIGGER_FALLING | IRQF_SHARED),
 	.x_flip		= false,
 	.y_flip		= true,
+	.regulator_en   = true,
 };
 
 static struct i2c_board_info __initdata mop500_i2c3_devices_u8500[] = {
diff --git a/arch/arm/mach-ux500/board-mop500-wlan.c b/arch/arm/mach-ux500/board-mop500-wlan.c
index 1e5c24ba1ea..3b15a4ef695 100644
--- a/arch/arm/mach-ux500/board-mop500-wlan.c
+++ b/arch/arm/mach-ux500/board-mop500-wlan.c
@@ -14,8 +14,14 @@
 #include <plat/pincfg.h>
 #include "pins.h"
 #include <mach/cw1200_plat.h>
+#include <linux/clk.h>
 
 static void cw1200_release(struct device *dev);
+static int cw1200_clk_ctrl(const struct cw1200_platform_data *pdata,
+		bool enable);
+
+static int cw1200_power_ctrl(const struct cw1200_platform_data *pdata,
+		bool enable);
 
 static struct resource cw1200_href_resources[] = {
 	{
@@ -62,7 +68,9 @@ static struct resource cw1200_u9500_resources[] = {
 	},
 };
 
-static struct cw1200_platform_data cw1200_platform_data = { 0 };
+static struct cw1200_platform_data cw1200_platform_data = {
+	.clk_ctrl = cw1200_clk_ctrl,
+};
 
 static struct platform_device cw1200_device = {
 	.name = "cw1200_wlan",
@@ -73,6 +81,8 @@ static struct platform_device cw1200_device = {
 	},
 };
 
+static struct clk *clk_dev;
+
 const struct cw1200_platform_data *cw1200_get_platform_data(void)
 {
 	return &cw1200_platform_data;
@@ -107,6 +117,68 @@ static int cw1200_pins_enable(bool enable)
 	return ret;
 }
 
+static int cw1200_clk_ctrl(const struct cw1200_platform_data *pdata,
+		bool enable)
+{
+	static const char *clock_name = "sys_clk_out";
+	int ret = 0;
+
+	if (enable) {
+		clk_dev = clk_get(&cw1200_device.dev, clock_name);
+		if (IS_ERR(clk_dev)) {
+			ret = PTR_ERR(clk_dev);
+			dev_warn(&cw1200_device.dev,
+				"%s: Failed to get clk '%s': %d\n",
+				__func__, clock_name, ret);
+		} else {
+			ret = clk_enable(clk_dev);
+			if (ret) {
+				clk_put(clk_dev);
+				dev_warn(&cw1200_device.dev,
+					"%s: Failed to enable clk '%s': %d\n",
+					__func__, clock_name, ret);
+			}
+		}
+	} else {
+		clk_disable(clk_dev);
+		clk_put(clk_dev);
+	}
+
+	return ret;
+}
+
+static int cw1200_power_ctrl(const struct cw1200_platform_data *pdata,
+		bool enable)
+{
+	static const char *vdd_name = "vdd";
+	struct regulator *vdd;
+	int ret = 0;
+
+	vdd = regulator_get(&cw1200_device.dev, vdd_name);
+	if (IS_ERR(vdd)) {
+		ret = PTR_ERR(vdd);
+		dev_warn(&cw1200_device.dev,
+				"%s: Failed to get regulator '%s': %d\n",
+				__func__, vdd_name, ret);
+	} else {
+		if (enable)
+			ret = regulator_enable(vdd);
+		else
+			ret = regulator_disable(vdd);
+
+		if (ret) {
+			dev_warn(&cw1200_device.dev,
+					"%s: Failed to %s regulator '%s': %d\n",
+					__func__, enable ? "enable" : "disable",
+					vdd_name, ret);
+		}
+		regulator_put(vdd);
+	}
+	return  ret;
+}
+
+
+
 int __init mop500_wlan_init(void)
 {
 	int ret;
@@ -114,7 +186,9 @@ int __init mop500_wlan_init(void)
 	if (pins_for_u9500()) {
 		cw1200_device.num_resources = ARRAY_SIZE(cw1200_u9500_resources);
 		cw1200_device.resource = cw1200_u9500_resources;
-	} else if (machine_is_u8500() || machine_is_nomadik()) {
+	} else if (machine_is_u8500() ||
+			   machine_is_nomadik() ||
+			   machine_is_snowball()) {
 		cw1200_device.num_resources = ARRAY_SIZE(cw1200_href_resources);
 		cw1200_device.resource = cw1200_href_resources;
 	} else if (machine_is_hrefv60()) {
@@ -129,13 +203,19 @@ int __init mop500_wlan_init(void)
 		return -ENOTSUPP;
 	}
 
-	cw1200_platform_data.mmc_id = "mmc3";
+	if (machine_is_snowball())
+		cw1200_platform_data.mmc_id = "mmc2";
+	else
+		cw1200_platform_data.mmc_id = "mmc3";
 
 	cw1200_platform_data.reset = &cw1200_device.resource[0];
 	cw1200_platform_data.irq = &cw1200_device.resource[1];
 
 	cw1200_device.dev.release = cw1200_release;
 
+	if (machine_is_snowball())
+		cw1200_platform_data.power_ctrl = cw1200_power_ctrl;
+
 	ret = cw1200_pins_enable(true);
 	if (WARN_ON(ret))
 		return ret;
diff --git a/arch/arm/mach-ux500/board-mop500.c b/arch/arm/mach-ux500/board-mop500.c
index 11b4ad69f91..4420adf0bd9 100644
--- a/arch/arm/mach-ux500/board-mop500.c
+++ b/arch/arm/mach-ux500/board-mop500.c
@@ -157,7 +157,7 @@ static struct abx500_accdet_platform_data ab8500_accdet_pdata = {
 
 static struct gpio_keys_button snowball_key_array[] = {
 	{
-		.gpio           = 32,
+		.gpio           = SNOWBALL_USER_PB_GPIO,
 		.type           = EV_KEY,
 		.code           = KEY_1,
 		.desc           = "userpb",
@@ -166,7 +166,7 @@ static struct gpio_keys_button snowball_key_array[] = {
 		.wakeup         = 1,
 	},
 	{
-		.gpio           = 151,
+		.gpio           = SNOWBALL_J1_PIN_8_GPIO,
 		.type           = EV_KEY,
 		.code           = KEY_2,
 		.desc           = "extkb1",
@@ -175,7 +175,7 @@ static struct gpio_keys_button snowball_key_array[] = {
 		.wakeup         = 1,
 	},
 	{
-		.gpio           = 152,
+		.gpio           = SNOWBALL_J1_PIN_9_GPIO,
 		.type           = EV_KEY,
 		.code           = KEY_3,
 		.desc           = "extkb2",
@@ -184,16 +184,7 @@ static struct gpio_keys_button snowball_key_array[] = {
 		.wakeup         = 1,
 	},
 	{
-		.gpio           = 161,
-		.type           = EV_KEY,
-		.code           = KEY_4,
-		.desc           = "extkb3",
-		.active_low     = 1,
-		.debounce_interval = 50,
-		.wakeup         = 1,
-	},
-	{
-		.gpio           = 162,
+		.gpio           = SNOWBALL_J1_PIN_14_GPIO,
 		.type           = EV_KEY,
 		.code           = KEY_5,
 		.desc           = "extkb4",
@@ -463,6 +454,32 @@ static struct ske_keypad_platform_data mop500_ske_keypad_data = {
 #endif
 
 
+#ifdef CONFIG_REGULATOR_FIXED_VOLTAGE
+/*
+ * GPIO-regulator wlan vbat data
+ */
+static struct fixed_voltage_config snowball_gpio_wlan_vbat_data = {
+	.supply_name		= "WLAN-VBAT",
+	.gpio			= SNOWBALL_EN_3V6_GPIO,
+	.microvolts		= 3600000,
+	.enable_high		= 1,
+	.init_data		= &gpio_wlan_vbat_regulator,
+	.startup_delay		= 3500, /* Startup time */
+};
+
+/*
+ * GPIO-regulator en 3v3 vbat data
+ */
+
+static struct fixed_voltage_config snowball_gpio_en_3v3_data = {
+	.supply_name		= "EN-3V3",
+	.gpio			= SNOWBALL_EN_3V3_ETH_GPIO,
+	.microvolts		= 3300000,
+	.enable_high		= 1,
+	.init_data		= &gpio_en_3v3_regulator,
+	.startup_delay		= 5000, /* 1200us according to data sheet */
+};
+#endif
 
 /*
  * TC35892
@@ -615,6 +632,24 @@ static void __init mop500_i2c_init(void)
 	db8500_add_i2c3(&u8500_i2c3_data);
 }
 
+#ifdef CONFIG_REGULATOR_FIXED_VOLTAGE
+static struct platform_device snowball_gpio_wlan_vbat_regulator_device = {
+	.name	= "reg-fixed-voltage",
+	.id	= 0,
+	.dev	= {
+		.platform_data	= &snowball_gpio_wlan_vbat_data,
+	},
+};
+
+static struct platform_device snowball_gpio_en_3v3_regulator_device = {
+	.name	= "reg-fixed-voltage",
+	.id	= 1,
+	.dev	= {
+		.platform_data	= &snowball_gpio_en_3v3_data,
+	},
+};
+#endif
+
 #ifdef CONFIG_UX500_GPIO_KEYS
 static struct gpio_keys_button mop500_gpio_keys[] = {
 	{
@@ -861,17 +896,20 @@ static struct cryp_platform_data u8500_cryp1_platform_data = {
 	}
 };
 
+static struct stedma40_chan_cfg u8500_hash_dma_cfg_tx = {
+	.dir = STEDMA40_MEM_TO_PERIPH,
+	.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
+	.dst_dev_type = DB8500_DMA_DEV50_HAC1_TX,
+	.src_info.data_width = STEDMA40_WORD_WIDTH,
+	.dst_info.data_width = STEDMA40_WORD_WIDTH,
+	.mode = STEDMA40_MODE_LOGICAL,
+	.src_info.psize = STEDMA40_PSIZE_LOG_16,
+	.dst_info.psize = STEDMA40_PSIZE_LOG_16,
+};
+
 static struct hash_platform_data u8500_hash1_platform_data = {
-	.mem_to_engine = {
-		.dir = STEDMA40_MEM_TO_PERIPH,
-		.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
-		.dst_dev_type = DB8500_DMA_DEV50_HAC1_TX,
-		.src_info.data_width = STEDMA40_WORD_WIDTH,
-		.dst_info.data_width = STEDMA40_WORD_WIDTH,
-		.mode = STEDMA40_MODE_LOGICAL,
-		.src_info.psize = STEDMA40_PSIZE_LOG_16,
-		.dst_info.psize = STEDMA40_PSIZE_LOG_16,
-	},
+	.mem_to_engine = &u8500_hash_dma_cfg_tx,
+	.dma_filter = stedma40_filter,
 };
 
 /* add any platform devices here - TODO */
@@ -1123,6 +1161,10 @@ static struct platform_device *snowball_platform_devs[] __initdata = {
 	&ux500_hwmem_device,
 	&snowball_led_dev,
 	&snowball_key_dev,
+#ifdef CONFIG_REGULATOR_FIXED_VOLTAGE
+	&snowball_gpio_en_3v3_regulator_device,
+	&snowball_gpio_wlan_vbat_regulator_device,
+#endif
 	&snowball_sbnet_dev,
 	&ux500_mcde_device,
 	&ux500_b2r2_device,
@@ -1158,9 +1200,10 @@ static void __init mop500_init_machine(void)
 	hsi_register_board_info(u8500_hsi_devices,
 				ARRAY_SIZE(u8500_hsi_devices));
 #endif
+#ifdef CONFIG_LEDS_PWM
 	if (uib_is_stuib())
 		u8500_leds_data.num_leds = 2;
-
+#endif
 	if (machine_is_snowball()) {
 		platform_add_devices(snowball_platform_devs,
 					ARRAY_SIZE(snowball_platform_devs));
diff --git a/arch/arm/mach-ux500/board-mop500.h b/arch/arm/mach-ux500/board-mop500.h
index 36dd0a17adb..53a04eee085 100644
--- a/arch/arm/mach-ux500/board-mop500.h
+++ b/arch/arm/mach-ux500/board-mop500.h
@@ -11,16 +11,6 @@
 #include <linux/mfd/ab8500/gpio.h>
 #include <mach/gpio.h>
 
-/* Snowball GPIO for MMC card */
-#define SNOWBALL_SDMMC_EN_GPIO		217
-#define SNOWBALL_SDMMC_1V8_3V_GPIO	228
-#define SNOWBALL_SDMMC_CD_GPIO		218
-
-/* Snowball specific GPIO assignments, this board has no GPIO expander */
-#define SNOWBALL_ACCEL_INT1_GPIO	163
-#define SNOWBALL_ACCEL_INT2_GPIO	164
-#define SNOWBALL_MAGNET_DRDY_GPIO	165
-
 /* HREFv60-specific GPIO assignments, this board has no GPIO expander */
 #define HREFV60_TOUCH_RST_GPIO		143
 #define HREFV60_PROX_SENSE_GPIO		217
@@ -60,6 +50,16 @@
 #define GPIO_SDMMC_EN			MOP500_EGPIO(17)
 #define GPIO_SDMMC_1V8_3V_SEL		MOP500_EGPIO(18)
 
+/* Snowball GPIO for MMC card */
+#define SNOWBALL_SDMMC_EN_GPIO		217
+#define SNOWBALL_SDMMC_1V8_3V_GPIO	228
+#define SNOWBALL_SDMMC_CD_GPIO		218
+
+/* Snowball specific GPIO assignments, this board has no GPIO expander */
+#define SNOWBALL_ACCEL_INT1_GPIO	163
+#define SNOWBALL_ACCEL_INT2_GPIO	164
+#define SNOWBALL_MAGNET_DRDY_GPIO	165
+
 /*Snowball AB8500 GPIO */
 #define SNOWBALL_VSMPS2_1V8_GPIO	AB8500_PIN_GPIO1	/* SYSCLKREQ2/GPIO1 */
 #define SNOWBALL_PM_GPIO1_GPIO		AB8500_PIN_GPIO2	/* SYSCLKREQ3/GPIO2 */
@@ -69,6 +69,12 @@
 #define SNOWBALL_PME_ETH_GPIO		AB8500_PIN_GPIO24	/* SYSCLKREQ7/GPIO24 */
 #define SNOWBALL_EN_3V3_ETH_GPIO	AB8500_PIN_GPIO26	/* GPIO26 */
 
+/*Snowball buttons GPIO */
+#define SNOWBALL_USER_PB_GPIO		32
+#define SNOWBALL_J1_PIN_8_GPIO		151 /* AP_GPIO151 */
+#define SNOWBALL_J1_PIN_9_GPIO		152 /* AP_GPIO152 */
+#define SNOWBALL_J1_PIN_14_GPIO		162 /* AP_GPIO162 */
+
 struct i2c_board_info;
 
 extern void mop500_sdi_init(void);
diff --git a/arch/arm/mach-ux500/board-u5500-regulators.c b/arch/arm/mach-ux500/board-u5500-regulators.c
index eae7e4fee22..7dc77bd4074 100644
--- a/arch/arm/mach-ux500/board-u5500-regulators.c
+++ b/arch/arm/mach-ux500/board-u5500-regulators.c
@@ -117,9 +117,7 @@ ab5500_regulator_init_data[AB5500_NUM_REGULATORS] = {
 	},
 	[AB5500_LDO_SIM] = {
 		.constraints = {
-			.boot_on	= 1,
-			.always_on	= 1,
-			.min_uV		= 2900000,
+			.min_uV		= 1875000,
 			.max_uV		= 2900000,
 			.apply_uV	= 1,
 			.valid_ops_mask	= REGULATOR_CHANGE_VOLTAGE |
diff --git a/arch/arm/mach-ux500/board-u5500-sdi.c b/arch/arm/mach-ux500/board-u5500-sdi.c
index 475e41e0628..ac2a7aac93d 100644
--- a/arch/arm/mach-ux500/board-u5500-sdi.c
+++ b/arch/arm/mach-ux500/board-u5500-sdi.c
@@ -66,6 +66,11 @@ static struct mmci_platform_data u5500_sdi0_data = {
 
 static int u5500_sdi1_ios_handler(struct device *dev, struct mmc_ios *ios)
 {
+	static int power_mode = -1;
+
+	if (power_mode == ios->power_mode)
+		return 0;
+
 	switch (ios->power_mode) {
 	case MMC_POWER_UP:
 	case MMC_POWER_ON:
@@ -85,6 +90,7 @@ static int u5500_sdi1_ios_handler(struct device *dev, struct mmc_ios *ios)
 		break;
 	}
 
+	power_mode = ios->power_mode;
 	return 0;
 }
 
diff --git a/arch/arm/mach-ux500/board-u5500.c b/arch/arm/mach-ux500/board-u5500.c
index b2056bd28bb..9b615de8fac 100644
--- a/arch/arm/mach-ux500/board-u5500.c
+++ b/arch/arm/mach-ux500/board-u5500.c
@@ -530,17 +530,20 @@ static struct cryp_platform_data u5500_cryp1_platform_data = {
 	}
 };
 
+static struct stedma40_chan_cfg u5500_hash_dma_cfg_tx = {
+	.dir = STEDMA40_MEM_TO_PERIPH,
+	.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
+	.dst_dev_type = DB5500_DMA_DEV50_HASH1_TX,
+	.src_info.data_width = STEDMA40_WORD_WIDTH,
+	.dst_info.data_width = STEDMA40_WORD_WIDTH,
+	.mode = STEDMA40_MODE_LOGICAL,
+	.src_info.psize = STEDMA40_PSIZE_LOG_16,
+	.dst_info.psize = STEDMA40_PSIZE_LOG_16,
+};
+
 static struct hash_platform_data u5500_hash1_platform_data = {
-	.mem_to_engine = {
-.dir = STEDMA40_MEM_TO_PERIPH,
-.src_dev_type = STEDMA40_DEV_SRC_MEMORY,
-.dst_dev_type = DB5500_DMA_DEV50_HASH1_TX,
-.src_info.data_width = STEDMA40_WORD_WIDTH,
-.dst_info.data_width = STEDMA40_WORD_WIDTH,
-.mode = STEDMA40_MODE_LOGICAL,
-.src_info.psize = STEDMA40_PSIZE_LOG_16,
-.dst_info.psize = STEDMA40_PSIZE_LOG_16,
-},
+	.mem_to_engine = &u5500_hash_dma_cfg_tx,
+	.dma_filter = stedma40_filter,
 };
 
 static struct platform_device *u5500_platform_devices[] __initdata = {
diff --git a/arch/arm/mach-ux500/cpu-db8500.c b/arch/arm/mach-ux500/cpu-db8500.c
index d2def0828a6..67330b8dd24 100644
--- a/arch/arm/mach-ux500/cpu-db8500.c
+++ b/arch/arm/mach-ux500/cpu-db8500.c
@@ -179,82 +179,3 @@ void __init u8500_init_devices(void)
 
 	return ;
 }
-
-#ifdef CONFIG_SYS_SOC
-#define U8500_BB_UID_BASE (U8500_BACKUPRAM1_BASE + 0xFC0)
-#define U8500_BB_UID_LENGTH 5
-
-static ssize_t ux500_get_machine(char *buf, struct sysfs_soc_info *si)
-{
-	return sprintf(buf, "DB%2x00\n", dbx500_id.partnumber);
-}
-
-static ssize_t ux500_get_soc_id(char *buf, struct sysfs_soc_info *si)
-{
-	void __iomem *uid_base;
-	int i;
-	ssize_t sz = 0;
-
-	if (dbx500_id.partnumber == 0x85) {
-		uid_base = __io_address(U8500_BB_UID_BASE);
-		for (i = 0; i < U8500_BB_UID_LENGTH; i++)
-			sz += sprintf(buf + sz, "%08x", readl(uid_base + i * sizeof(u32)));
-		sz += sprintf(buf + sz, "\n");
-	}
-	else {
-		/* Don't know where it is located for U5500 */
-		sz = sprintf(buf, "N/A\n");
-	}
-
-	return sz;
-}
-
-static ssize_t ux500_get_revision(char *buf, struct sysfs_soc_info *si)
-{
-	unsigned int rev = dbx500_id.revision;
-
-	if (rev == 0x01)
-		return sprintf(buf, "%s\n", "ED");
-	else if (rev >= 0xA0)
-		return sprintf(buf, "%d.%d\n" , (rev >> 4) - 0xA + 1, rev & 0xf);
-
-	return sprintf(buf, "%s", "Unknown\n");
-}
-
-static ssize_t ux500_get_process(char *buf, struct sysfs_soc_info *si)
-{
-	if (dbx500_id.process == 0x00)
-		return sprintf(buf, "Standard\n");
-
-	return sprintf(buf, "%02xnm\n", dbx500_id.process);
-}
-
-static ssize_t ux500_get_reset_code(char *buf, struct sysfs_soc_info *si)
-{
-	return sprintf(buf, "0x%04x\n", prcmu_get_reset_code());
-}
-
-static ssize_t ux500_get_reset_reason(char *buf, struct sysfs_soc_info *si)
-{
-	return sprintf(buf, "%s\n",
-		reboot_reason_string(prcmu_get_reset_code()));
-}
-
-static struct sysfs_soc_info soc_info[] = {
-	SYSFS_SOC_ATTR_CALLBACK("machine", ux500_get_machine),
-	SYSFS_SOC_ATTR_VALUE("family", "Ux500"),
-	SYSFS_SOC_ATTR_CALLBACK("soc_id", ux500_get_soc_id),
-	SYSFS_SOC_ATTR_CALLBACK("revision", ux500_get_revision),
-	SYSFS_SOC_ATTR_CALLBACK("process", ux500_get_process),
-	SYSFS_SOC_ATTR_CALLBACK("reset_code", ux500_get_reset_code),
-	SYSFS_SOC_ATTR_CALLBACK("reset_reason", ux500_get_reset_reason),
-};
-
-static int __init ux500_sys_soc_init(void)
-{
-	return register_sysfs_soc(soc_info, ARRAY_SIZE(soc_info));
-}
-
-module_init(ux500_sys_soc_init);
-#endif
-
diff --git a/arch/arm/mach-ux500/cpu.c b/arch/arm/mach-ux500/cpu.c
index bfdebbeab3e..ec106bbdc48 100644
--- a/arch/arm/mach-ux500/cpu.c
+++ b/arch/arm/mach-ux500/cpu.c
@@ -9,6 +9,7 @@
 #include <linux/io.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/sys_soc.h>
 #include <linux/clksrc-dbx500-prcmu.h>
 #include <linux/mfd/dbx500-prcmu.h>
 
@@ -155,3 +156,82 @@ static int ux500_l2x0_init(void)
 }
 early_initcall(ux500_l2x0_init);
 #endif
+
+#ifdef CONFIG_SYS_SOC
+#define U8500_BB_UID_BASE (U8500_BACKUPRAM1_BASE + 0xFC0)
+#define U8500_BB_UID_LENGTH 5
+
+static ssize_t ux500_get_machine(char *buf, struct sysfs_soc_info *si)
+{
+	return sprintf(buf, "DB%2x00\n", dbx500_id.partnumber);
+}
+
+static ssize_t ux500_get_soc_id(char *buf, struct sysfs_soc_info *si)
+{
+	void __iomem *uid_base;
+	int i;
+	ssize_t sz = 0;
+
+	if (dbx500_id.partnumber == 0x85) {
+		uid_base = __io_address(U8500_BB_UID_BASE);
+		for (i = 0; i < U8500_BB_UID_LENGTH; i++)
+			sz += sprintf(buf + sz, "%08x",
+					readl(uid_base + i * sizeof(u32)));
+		sz += sprintf(buf + sz, "\n");
+	} else {
+		/* Don't know where it is located for U5500 */
+		sz = sprintf(buf, "N/A\n");
+	}
+
+	return sz;
+}
+
+static ssize_t ux500_get_revision(char *buf, struct sysfs_soc_info *si)
+{
+	unsigned int rev = dbx500_id.revision;
+
+	if (rev == 0x01)
+		return sprintf(buf, "%s\n", "ED");
+	else if (rev >= 0xA0)
+		return sprintf(buf, "%d.%d\n" ,
+				(rev >> 4) - 0xA + 1, rev & 0xf);
+
+	return sprintf(buf, "%s", "Unknown\n");
+}
+
+static ssize_t ux500_get_process(char *buf, struct sysfs_soc_info *si)
+{
+	if (dbx500_id.process == 0x00)
+		return sprintf(buf, "Standard\n");
+
+	return sprintf(buf, "%02xnm\n", dbx500_id.process);
+}
+
+static ssize_t ux500_get_reset_code(char *buf, struct sysfs_soc_info *si)
+{
+	return sprintf(buf, "0x%04x\n", prcmu_get_reset_code());
+}
+
+static ssize_t ux500_get_reset_reason(char *buf, struct sysfs_soc_info *si)
+{
+	return sprintf(buf, "%s\n",
+		reboot_reason_string(prcmu_get_reset_code()));
+}
+
+static struct sysfs_soc_info soc_info[] = {
+	SYSFS_SOC_ATTR_CALLBACK("machine", ux500_get_machine),
+	SYSFS_SOC_ATTR_VALUE("family", "Ux500"),
+	SYSFS_SOC_ATTR_CALLBACK("soc_id", ux500_get_soc_id),
+	SYSFS_SOC_ATTR_CALLBACK("revision", ux500_get_revision),
+	SYSFS_SOC_ATTR_CALLBACK("process", ux500_get_process),
+	SYSFS_SOC_ATTR_CALLBACK("reset_code", ux500_get_reset_code),
+	SYSFS_SOC_ATTR_CALLBACK("reset_reason", ux500_get_reset_reason),
+};
+
+static int __init ux500_sys_soc_init(void)
+{
+	return register_sysfs_soc(soc_info, ARRAY_SIZE(soc_info));
+}
+
+module_init(ux500_sys_soc_init);
+#endif
diff --git a/arch/arm/mach-ux500/dma-db8500.c b/arch/arm/mach-ux500/dma-db8500.c
index e4e33c5b655..5a6ff377f03 100644
--- a/arch/arm/mach-ux500/dma-db8500.c
+++ b/arch/arm/mach-ux500/dma-db8500.c
@@ -231,6 +231,9 @@ static struct stedma40_platform_data dma40_plat_data = {
 	/* Audio is using physical channel 2 from MMDSP */
 	.disabled_channels = {2, -1},
 	.use_esram_lcla = true,
+	/* Physical channels for which HW LLI should not be used */
+	.soft_lli_chans = NULL,
+	.num_of_soft_lli_chans = 0,
 };
 
 #ifdef CONFIG_UX500_CONTEXT
diff --git a/arch/arm/mach-ux500/include/mach/crypto-ux500.h b/arch/arm/mach-ux500/include/mach/crypto-ux500.h
index 9d1e1c52c13..80c4620d633 100644
--- a/arch/arm/mach-ux500/include/mach/crypto-ux500.h
+++ b/arch/arm/mach-ux500/include/mach/crypto-ux500.h
@@ -5,6 +5,7 @@
  * License terms: GNU General Public License (GPL) version 2
  */
 #ifndef _CRYPTO_UX500_H
+#include <linux/dmaengine.h>
 #include <plat/ste_dma40.h>
 
 struct cryp_platform_data {
@@ -13,7 +14,8 @@ struct cryp_platform_data {
 };
 
 struct hash_platform_data {
-	struct stedma40_chan_cfg mem_to_engine;
+	void *mem_to_engine;
+	bool (*dma_filter)(struct dma_chan *chan, void *filter_param);
 };
 
 #endif
diff --git a/arch/arm/mach-ux500/include/mach/mbox_channels-db5500.h b/arch/arm/mach-ux500/include/mach/mbox_channels-db5500.h
index 0b2fc604873..c29dfff0ff7 100644
--- a/arch/arm/mach-ux500/include/mach/mbox_channels-db5500.h
+++ b/arch/arm/mach-ux500/include/mach/mbox_channels-db5500.h
@@ -68,5 +68,13 @@ int mbox_channel_send(struct mbox_channel_msg *msg);
  */
 int mbox_channel_revoke_messages(u16 channel);
 
+/**
+ * mbox_channel_deregister - de-register given mailbox channel.
+ * @channel:    Mailbox channel number.
+ *
+ * Returns 0 on success or a negative error code on error.
+ */
+int mbox_channel_deregister(u16 channel);
+
 #endif /*INC_STE_MBOX_H*/
 
diff --git a/arch/arm/mach-ux500/pins.c b/arch/arm/mach-ux500/pins.c
index 4738103646d..c506080a115 100644
--- a/arch/arm/mach-ux500/pins.c
+++ b/arch/arm/mach-ux500/pins.c
@@ -12,6 +12,7 @@
 #include <linux/spinlock.h>
 #include <linux/err.h>
 #include <plat/pincfg.h>
+#include <linux/gpio.h>
 
 #include "pins.h"
 
@@ -87,6 +88,44 @@ void ux500_pins_put(struct ux500_pins *pins)
 	WARN_ON(!pins);
 }
 
+void __init ux500_offchip_gpio_init(struct ux500_pins *pins)
+{
+	int err;
+	int i;
+	int gpio;
+	int output;
+	int value;
+	pin_cfg_t cfg;
+
+	for (i=0; i < pins->num; i++) {
+		cfg = pins->cfg[i];
+		gpio = PIN_NUM(cfg);
+		output = PIN_DIR(cfg);
+		value = PIN_VAL(cfg);
+
+		err = gpio_request(gpio, "offchip_gpio_init");
+		if (err < 0) {
+			pr_err("pins: gpio_request for gpio=%d failed with"
+				"err: %d\n", gpio, err);
+			/* Pin already requested. Try to configure rest. */
+			continue;
+		}
+
+		if (!output) {
+			err = gpio_direction_input(gpio);
+			if (err < 0)
+				pr_err("pins: gpio_direction_input for gpio=%d"
+					"failed with err: %d\n", gpio, err);
+		} else {
+			err = gpio_direction_output(gpio, value);
+			if (err < 0)
+				pr_err("pins: gpio_direction_output for gpio="
+					"%d failed with err: %d\n", gpio, err);
+		}
+		gpio_free(gpio);
+	}
+}
+
 #ifdef CONFIG_DEBUG_FS
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
diff --git a/arch/arm/mach-ux500/pins.h b/arch/arm/mach-ux500/pins.h
index 0fa65cc6b96..0d36af2e7d9 100644
--- a/arch/arm/mach-ux500/pins.h
+++ b/arch/arm/mach-ux500/pins.h
@@ -36,6 +36,7 @@ struct ux500_pin_lookup {
 };
 
 void __init ux500_pins_add(struct ux500_pin_lookup *pl, size_t num);
+void __init ux500_offchip_gpio_init(struct ux500_pins *pins);
 struct ux500_pins *ux500_pins_get(const char *name);
 int ux500_pins_enable(struct ux500_pins *pins);
 int ux500_pins_disable(struct ux500_pins *pins);
diff --git a/arch/arm/mach-ux500/pm/context.c b/arch/arm/mach-ux500/pm/context.c
index b22e3ad7fd1..8c339ff948e 100644
--- a/arch/arm/mach-ux500/pm/context.c
+++ b/arch/arm/mach-ux500/pm/context.c
@@ -7,7 +7,6 @@
  * License terms: GNU General Public License (GPL) version 2
  *
  */
-
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/smp.h>
@@ -112,7 +111,9 @@
  * Periph clock cluster context
  */
 #define PRCC_BCK_EN		0x00
+#define PRCC_BCK_DIS		0x04
 #define PRCC_KCK_EN		0x08
+#define PRCC_KCK_DIS		0x08
 #define PRCC_BCK_STATUS		0x10
 #define PRCC_KCK_STATUS		0x14
 
@@ -206,7 +207,6 @@ static DEFINE_PER_CPU(u32 *, varm_registers_pointer);
 static DEFINE_PER_CPU(u32[128], varm_cp15_backup_stack);
 static DEFINE_PER_CPU(u32 *, varm_cp15_pointer);
 
-
 static ATOMIC_NOTIFIER_HEAD(context_ape_notifier_list);
 static ATOMIC_NOTIFIER_HEAD(context_arm_notifier_list);
 
@@ -271,10 +271,19 @@ static void restore_prcc(void)
 	for (i = 0; i < UX500_NR_PRCC_BANKS; i++) {
 		clk_enable(context_prcc[i].clk);
 
+		writel(~context_prcc[i].bus_clk,
+		       context_prcc[i].base + PRCC_BCK_DIS);
+		writel(~context_prcc[i].kern_clk,
+		       context_prcc[i].base + PRCC_KCK_DIS);
+
 		writel(context_prcc[i].bus_clk,
 		       context_prcc[i].base + PRCC_BCK_EN);
 		writel(context_prcc[i].kern_clk,
 		       context_prcc[i].base + PRCC_KCK_EN);
+		/*
+		 * Consider having a while over KCK/BCK_STATUS
+		 * to check that all clocks get disabled/enabled
+		 */
 
 		clk_disable(context_prcc[i].clk);
 	}
@@ -372,7 +381,6 @@ static void restore_tpiu(void)
  *
  * This is per cpu so it needs to be called for each one.
  */
-
 static void save_gic_if_cpu(struct context_gic_cpu *c_gic_cpu)
 {
 	c_gic_cpu->ctrl     = readl_relaxed(c_gic_cpu->base + GIC_CPU_CTRL);
@@ -399,7 +407,6 @@ static void restore_gic_if_cpu(struct context_gic_cpu *c_gic_cpu)
  *
  * Save SPI (Shared Peripheral Interrupt) settings, IRQ 32-159.
  */
-
 static void save_gic_dist_common(void)
 {
 	int i;
@@ -437,7 +444,6 @@ static void save_gic_dist_common(void)
  */
 static void restore_gic_dist_common(void)
 {
-
 	int i;
 
 	for (i = 0; i < GIC_DIST_CONFIG_COMMON_NUM; i++)
@@ -464,8 +470,6 @@ static void restore_gic_dist_common(void)
 	       context_gic_dist_common.base + GIC_DIST_ENABLE_NS);
 }
 
-
-
 /*
  * Save GIC Dist CPU registers
  *
@@ -507,7 +511,6 @@ static void save_gic_dist_cpu(struct context_gic_dist_cpu *c_gic)
  */
 static void restore_gic_dist_cpu(struct context_gic_dist_cpu *c_gic)
 {
-
 	int i;
 
 	for (i = 0; i < GIC_DIST_CONFIG_CPU_NUM; i++)
@@ -537,7 +540,6 @@ static void restore_gic_dist_cpu(struct context_gic_dist_cpu *c_gic)
  */
 void context_gic_dist_disable_unneeded_irqs(void)
 {
-
 	writel(0xffffffff,
 	       context_gic_dist_common.base +
 	       GIC_DIST_ENABLE_CLEAR_0);
@@ -558,7 +560,6 @@ void context_gic_dist_disable_unneeded_irqs(void)
 	writel(0xffffffff,
 	       context_gic_dist_common.base +
 	       GIC_DIST_ENABLE_CLEAR_128);
-
 }
 
 static void save_scu(void)
@@ -611,7 +612,6 @@ void context_vape_save(void)
 	save_tpiu();
 
 	save_prcc();
-
 }
 
 /*
@@ -700,7 +700,6 @@ void context_gpio_restore(void)
 		writel(gpio_save[i][6], gpio_bankaddr[i] + NMK_GPIO_SLPC);
 
 	}
-
 }
 
 /*
@@ -759,9 +758,7 @@ void context_gpio_mux_safe_switch(bool begin)
 			writel(rwimsc[i], gpio_bankaddr[i] + NMK_GPIO_RWIMSC);
 			writel(fwimsc[i], gpio_bankaddr[i] + NMK_GPIO_FWIMSC);
 		}
-
 	}
-
 }
 
 /*
@@ -803,7 +800,6 @@ void context_varm_restore_common(void)
  */
 void context_varm_save_core(void)
 {
-
 	int cpu = smp_processor_id();
 
 	atomic_notifier_call_chain(&context_arm_notifier_list,
@@ -816,7 +812,6 @@ void context_varm_save_core(void)
 	save_gic_if_cpu(&per_cpu(context_gic_cpu, cpu));
 	save_gic_dist_cpu(&per_cpu(context_gic_dist_cpu, cpu));
 	context_save_cp15_registers(&per_cpu(varm_cp15_pointer, cpu));
-
 }
 
 /*
@@ -837,7 +832,6 @@ void context_varm_restore_core(void)
 
 	atomic_notifier_call_chain(&context_arm_notifier_list,
 				   CONTEXT_ARM_CORE_RESTORE, NULL);
-
 }
 
 /*
@@ -901,7 +895,6 @@ static int __init context_init(void)
 	writel(virt_to_phys(ux500_backup_ptr),
 	       IO_ADDRESS(U8500_EXT_RAM_LOC_BACKUPRAM_ADDR));
 
-
 	if (cpu_is_u5500()) {
 		writel(IO_ADDRESS(U5500_PUBLIC_BOOT_ROM_BASE),
 		       IO_ADDRESS(U8500_CPU0_BACKUPRAM_ADDR_PUBLIC_BOOT_ROM_LOG_ADDR));
diff --git a/arch/arm/mach-ux500/product.c b/arch/arm/mach-ux500/product.c
index 5e8eba9b16a..8fee45c411d 100644
--- a/arch/arm/mach-ux500/product.c
+++ b/arch/arm/mach-ux500/product.c
@@ -30,13 +30,12 @@ static struct tee_product_config product_config;
 bool ux500_jtag_enabled(void)
 {
 #ifdef CONFIG_UX500_DEBUG_NO_LAUTERBACH
-        return false;
+	return false;
 #else
-        if (cpu_is_u5500())
-                return readl_relaxed(__io_address(U5500_PRCMU_DBG_PWRCTRL))
-                        & PRCMU_DBG_PWRCTRL_A9DBGCLKEN;
-
-        if (cpu_is_u8500())
+	if (cpu_is_u5500())
+		return readl_relaxed(__io_address(U5500_PRCMU_DBG_PWRCTRL))
+			& PRCMU_DBG_PWRCTRL_A9DBGCLKEN;
+	if (cpu_is_u8500())
 		return (product_config.rt_flags & TEE_RT_FLAGS_JTAG_ENABLED) ==
 			TEE_RT_FLAGS_JTAG_ENABLED;
 
@@ -96,8 +95,35 @@ static int __init product_detect(void)
 		goto error1;
 	}
 
+	switch(product_config.product_id) {
+	case TEE_PRODUCT_ID_8400:
+		pr_info("ux500-product: u8400 detected\n");
+		break;
+	case TEE_PRODUCT_ID_8500:
+		pr_info("ux500-product: u8500 detected\n");
+		break;
+	case TEE_PRODUCT_ID_9500:
+		pr_info("ux500-product: u9500 detected\n");
+		break;
+	case TEE_PRODUCT_ID_5500:
+		pr_info("ux500-product: u5500 detected\n");
+		break;
+	case TEE_PRODUCT_ID_7400:
+		pr_info("ux500-product: u7400 detected\n");
+		break;
+	case TEE_PRODUCT_ID_8500C:
+		pr_info("ux500-product: u8500C detected\n");
+		break;
+	case TEE_PRODUCT_ID_UNKNOWN:
+	default:
+		pr_info("ux500-product: UNKNOWN! (0x%x) detected\n",
+			product_config.product_id);
+		break;
+	}
 	pr_info("ux500-product: JTAG is %s\n",
 		ux500_jtag_enabled()? "enabled" : "disabled");
+
+
 error1:
 	(void) teec_finalize_context(&context);
 error0:
diff --git a/arch/arm/mach-ux500/usb.c b/arch/arm/mach-ux500/usb.c
index 5ba1b4373aa..5baae5f06aa 100644
--- a/arch/arm/mach-ux500/usb.c
+++ b/arch/arm/mach-ux500/usb.c
@@ -96,7 +96,47 @@ static u64 ux500_musb_dmamask = DMA_BIT_MASK(0);
 #endif
 static struct ux500_pins *usb_gpio_pins;
 
+/**
+ * Fifo mode
+ * Sum of maxpacket <= 12 KB
+ * As ux500 provides 12 KB buffer size only
+ *
+ * Enable Double buffer for Mass Storage Class
+ * endpoint.
+ */
+static struct musb_fifo_cfg ux500_mode_cfg[] = {
+{ .hw_ep_num =  1, .style = FIFO_TX,   .maxpacket = 512,  .mode = BUF_DOUBLE, },
+{ .hw_ep_num =  1, .style = FIFO_RX,   .maxpacket = 512,  .mode = BUF_DOUBLE, },
+{ .hw_ep_num =  2, .style = FIFO_TX,   .maxpacket = 512,  .mode = BUF_DOUBLE, },
+{ .hw_ep_num =  2, .style = FIFO_RX,   .maxpacket = 512,  .mode = BUF_DOUBLE, },
+{ .hw_ep_num =  3, .style = FIFO_TX,   .maxpacket = 512, },
+{ .hw_ep_num =  3, .style = FIFO_RX,   .maxpacket = 512, },
+{ .hw_ep_num =  4, .style = FIFO_TX,   .maxpacket = 512, },
+{ .hw_ep_num =  4, .style = FIFO_RX,   .maxpacket = 512, },
+{ .hw_ep_num =  5, .style = FIFO_TX,   .maxpacket = 512, },
+{ .hw_ep_num =  5, .style = FIFO_RX,   .maxpacket = 512, },
+{ .hw_ep_num =  6, .style = FIFO_TX,   .maxpacket = 32, },
+{ .hw_ep_num =  6, .style = FIFO_RX,   .maxpacket = 32, },
+{ .hw_ep_num =  7, .style = FIFO_TX,   .maxpacket = 32, },
+{ .hw_ep_num =  7, .style = FIFO_RX,   .maxpacket = 32, },
+{ .hw_ep_num =  8, .style = FIFO_TX,   .maxpacket = 32, },
+{ .hw_ep_num =  8, .style = FIFO_RX,   .maxpacket = 32, },
+{ .hw_ep_num =  9, .style = FIFO_TX,   .maxpacket = 32, },
+{ .hw_ep_num =  9, .style = FIFO_RX,   .maxpacket = 32, },
+{ .hw_ep_num = 10, .style = FIFO_TX,   .maxpacket = 32, },
+{ .hw_ep_num = 10, .style = FIFO_RX,   .maxpacket = 32, },
+{ .hw_ep_num = 11, .style = FIFO_TX,   .maxpacket = 32, },
+{ .hw_ep_num = 11, .style = FIFO_RX,   .maxpacket = 32, },
+{ .hw_ep_num = 12, .style = FIFO_TX,   .maxpacket = 32, },
+{ .hw_ep_num = 12, .style = FIFO_RX,   .maxpacket = 32, },
+{ .hw_ep_num = 13, .style = FIFO_RXTX, .maxpacket = 512, },
+{ .hw_ep_num = 14, .style = FIFO_RXTX, .maxpacket = 1024, },
+{ .hw_ep_num = 15, .style = FIFO_RXTX, .maxpacket = 1024, },
+};
+
 static struct musb_hdrc_config musb_hdrc_config = {
+	.fifo_cfg       = ux500_mode_cfg, /* Fifo configuration */
+	.fifo_cfg_size  = ARRAY_SIZE(ux500_mode_cfg),
 	.multipoint	= true,
 	.dyn_fifo	= true,
 	.num_eps	= 16,
diff --git a/arch/arm/plat-nomadik/include/plat/ste_dma40.h b/arch/arm/plat-nomadik/include/plat/ste_dma40.h
index ec716f24bac..c97d61425db 100644
--- a/arch/arm/plat-nomadik/include/plat/ste_dma40.h
+++ b/arch/arm/plat-nomadik/include/plat/ste_dma40.h
@@ -141,6 +141,12 @@ struct stedma40_chan_cfg {
  * @memcpy_conf_phy: default configuration of physical channel memcpy
  * @memcpy_conf_log: default configuration of logical channel memcpy
  * @disabled_channels: A vector, ending with -1, that marks physical channels
+ * @soft_lli_chans: A vector, that marks physical channels will use LLI by SW
+ * 		    which avoids HW bug that exists in some versions of the
+ * 		    controller. SoftLLI introduces relink overhead that could
+ *		    impact performace for certain use cases.
+ * @num_of_soft_lli_chans: The number of channels that needs to be configured
+ *			   to use SoftLLI.
  * @use_esram_lcla: flag for mapping the lcla into esram region
  * that are for different reasons not available for the driver.
  */
@@ -153,29 +159,13 @@ struct stedma40_platform_data {
 	struct stedma40_chan_cfg	*memcpy_conf_phy;
 	struct stedma40_chan_cfg	*memcpy_conf_log;
 	int				 disabled_channels[STEDMA40_MAX_PHYS];
+	int				*soft_lli_chans;
+	int				 num_of_soft_lli_chans;
 	bool				 use_esram_lcla;
 };
 
 struct d40_desc;
 
-/**
- * struct stedma40_cyclic_desc - Cyclic DMA descriptor
- * @d40d: DMA driver internal descriptor
- * @period_callback: callback to be called after every link/period if
- *		     the DMA_PREP_INTERRUPT flag is used when preparing
- *		     the transaction
- * @period_callback_param: handle passed to the period_callback
- *
- * A pointer to a structure of this type is returned from the
- * stedma40_cyclic_prep_sg() function.  The period_callback and
- * period_callback_param members can be set by the client.
- */
-struct stedma40_cyclic_desc {
-	struct d40_desc *d40d;
-	dma_async_tx_callback period_callback;
-	void *period_callback_param;
-};
-
 int stedma40_set_dev_addr(struct dma_chan *chan,
 			  dma_addr_t src_dev_addr,
 			  dma_addr_t dst_dev_addr);
@@ -199,52 +189,6 @@ dma_addr_t stedma40_get_src_addr(struct dma_chan *chan);
 dma_addr_t stedma40_get_dst_addr(struct dma_chan *chan);
 
 /**
- * stedma40_cyclic_prep_sg - prepare a cyclic DMA transfer
- * @chan: the DMA channel to prepare
- * @sgl: scatter list
- * @sg_len: number of links in the scatter list
- * @direction: transfer direction, to or from device
- * @dma_flags: DMA_PREP_INTERRUPT if a callback is required after every link.
- *	       See period_callback in struct stedma40_cyclic_desc.
- *
- * Must be called before trying to start a cyclic DMA transfer.  Returns
- * ERR_PTR(-errno) on failure.
- */
-struct stedma40_cyclic_desc *
-stedma40_cyclic_prep_sg(struct dma_chan *chan,
-			struct scatterlist *sgl,
-			unsigned int sg_len,
-			enum dma_data_direction direction,
-			unsigned long dma_flags);
-
-/**
- * stedma40_cyclic_start - start the cyclic DMA transfer
- * @chan: the DMA channel to start
- *
- * The cyclic DMA must have been prepared earlier with
- * stedma40_cyclic_prep_sg().
- */
-int stedma40_cyclic_start(struct dma_chan *chan);
-
-/**
- * stedma40_cyclic_stop() - stop the cyclic DMA transfer
- * @chan: the DMA channel to stop
- *
- * Stops a cyclic DMA transfer which was previously started with
- * stedma40_cyclic_start().
- */
-void stedma40_cyclic_stop(struct dma_chan *chan);
-
-/**
- * stedma40_cyclic_free() - free cyclic DMA resources
- * @chan: the DMA channel
- *
- * Must be called to free any resources used for cyclic DMA which have been
- * allocated in stedma40_cyclic_prep_sg().
- */
-void stedma40_cyclic_free(struct dma_chan *chan);
-
-/**
  * setdma40_residue() - Returna the remaining bytes to transfer.
  *
  * @chan: dmaengine handle
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 75935af43f2..a47a5837737 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -12,11 +12,9 @@ config CLKSRC_DBX500_PRCMU
 	  Use the always on PRCMU Timer as clocksource
 
 config CLKSRC_DBX500_PRCMU_SCHED_CLOCK
-	bool "Clocksource PRCMU Timer sched_clock"
-	depends on (CLKSRC_DBX500_PRCMU && !NOMADIK_MTU_SCHED_CLOCK && \
-			!CLKSRC_DB5500_MTIMER_SCHED_CLOCK)
+	bool
+	depends on CLKSRC_DBX500_PRCMU
 	select HAVE_SCHED_CLOCK
-	default y
 	help
 	  Use the always on PRCMU Timer as sched_clock
 
@@ -28,10 +26,8 @@ config CLKSRC_DB5500_MTIMER
 	  Use the always on MTIMER as clocksource
 
 config CLKSRC_DB5500_MTIMER_SCHED_CLOCK
-	bool "Clocksource MTIMER sched_clock"
-	depends on (CLKSRC_DB5500_MTIMER && !NOMADIK_MTU_SCHED_CLOCK && \
-			!CLKSRC_DBX500_PRCMU_SCHED_CLOCK)
+	bool
+	depends on CLKSRC_DB5500_MTIMER
 	select HAVE_SCHED_CLOCK
-	default y
 	help
 	  Use the always on PRCMU Timer as sched_clock
diff --git a/drivers/cpufreq/cpufreq_interactive.c b/drivers/cpufreq/cpufreq_interactive.c
index d8f62f402bf..38a874ed074 100644
--- a/drivers/cpufreq/cpufreq_interactive.c
+++ b/drivers/cpufreq/cpufreq_interactive.c
@@ -137,9 +137,14 @@ static void cpufreq_interactive_timer(unsigned long data)
 
 	if (delta_idle > delta_time)
 		cpu_load = 0;
-	else
-		cpu_load = 100 * (delta_time - delta_idle) / delta_time;
+	else {
+		unsigned long long load;
+
+		load = 100llu * (delta_time - delta_idle);
+		do_div(load, delta_time);
 
+		cpu_load = load;
+	}
 	delta_idle = (unsigned int) cputime64_sub(now_idle,
 						pcpu->freq_change_time_in_idle);
 	delta_time = (unsigned int) cputime64_sub(pcpu->timer_run_time,
@@ -147,9 +152,14 @@ static void cpufreq_interactive_timer(unsigned long data)
 
 	if (delta_idle > delta_time)
 		load_since_change = 0;
-	else
-		load_since_change =
-			100 * (delta_time - delta_idle) / delta_time;
+	else {
+		unsigned long long load;
+
+		load = 100llu * (delta_time - delta_idle);
+		do_div(load, delta_time);
+
+		load_since_change = load;
+	}
 
 	/*
 	 * Choose greater of short-term load (since last idle timer
diff --git a/drivers/cpuidle/cpuidle-dbx500.c b/drivers/cpuidle/cpuidle-dbx500.c
index 59f74a1b98f..aeb56f97611 100644
--- a/drivers/cpuidle/cpuidle-dbx500.c
+++ b/drivers/cpuidle/cpuidle-dbx500.c
@@ -288,7 +288,7 @@ static int determine_sleep_state(u32 *sleep_time)
 
 	int cpu;
 	int max_depth;
-	bool power_state_req;
+	bool uart, modem, ape;
 
 	/* If first cpu to sleep, go to most shallow sleep state */
 	if (!is_last_cpu_running())
@@ -306,8 +306,6 @@ static int determine_sleep_state(u32 *sleep_time)
 			return CI_WFI;
 	}
 
-	power_state_req = power_state_active_is_enabled() ||
-		prcmu_is_ac_wake_requested();
 
 	(*sleep_time) = get_remaining_sleep_time(NULL, NULL);
 
@@ -324,6 +322,10 @@ static int determine_sleep_state(u32 *sleep_time)
 			max_depth = per_cpu(cpu_state, cpu)->gov_cstate;
 	}
 
+	uart = ux500_ci_dbg_force_ape_on();
+	ape = power_state_active_is_enabled();
+	modem = prcmu_is_ac_wake_requested();
+
 	for (i = max_depth; i > 0; i--) {
 
 		if ((*sleep_time) <= cstates[i].threshold)
@@ -331,8 +333,7 @@ static int determine_sleep_state(u32 *sleep_time)
 
 		if (cstates[i].APE == APE_OFF) {
 			/* This state says APE should be off */
-			if (power_state_req ||
-			    ux500_ci_dbg_force_ape_on())
+			if (ape || modem || uart)
 				continue;
 		}
 
@@ -340,7 +341,7 @@ static int determine_sleep_state(u32 *sleep_time)
 		break;
 	}
 
-	ux500_ci_dbg_register_reason(i, power_state_req,
+	ux500_ci_dbg_register_reason(i, ape, modem, uart,
 				     (*sleep_time),
 				     max_depth);
 	return max(CI_WFI, i);
diff --git a/drivers/cpuidle/cpuidle-dbx500_dbg.c b/drivers/cpuidle/cpuidle-dbx500_dbg.c
index 43cdcfdd241..16a8f66a40c 100644
--- a/drivers/cpuidle/cpuidle-dbx500_dbg.c
+++ b/drivers/cpuidle/cpuidle-dbx500_dbg.c
@@ -64,6 +64,8 @@ struct state_history {
 	u32 exit_counter;
 	ktime_t measure_begin;
 	int ape_blocked;
+	int uart_blocked;
+	int modem_blocked;
 	int time_blocked;
 	int both_blocked;
 	int gov_blocked;
@@ -86,6 +88,8 @@ static int verbose;
 
 static bool apidle_both_blocked;
 static bool apidle_ape_blocked;
+static bool apidle_modem_blocked;
+static bool apidle_uart_blocked;
 static bool apidle_time_blocked;
 static bool apidle_gov_blocked;
 
@@ -336,13 +340,15 @@ static void state_record_time(struct state_history *sh, int ctarget,
 	sh->states[sh->state].counter++;
 }
 
-void ux500_ci_dbg_register_reason(int idx, bool power_state_req,
+void ux500_ci_dbg_register_reason(int idx, bool ape, bool modem, bool uart,
 				  u32 time, u32 max_depth)
 {
 	if (cstates[idx].state == CI_IDLE && verbose) {
-		apidle_ape_blocked = power_state_req;
+		apidle_ape_blocked = ape;
+		apidle_uart_blocked = uart;
+		apidle_modem_blocked = modem;
 		apidle_time_blocked = time < cstates[idx + 1].threshold;
-		apidle_both_blocked = power_state_req && apidle_time_blocked;
+		apidle_both_blocked = (ape || uart || modem) && apidle_time_blocked;
 		apidle_gov_blocked = cstates[max_depth].state == CI_IDLE;
 	}
 }
@@ -369,6 +375,10 @@ void ux500_ci_dbg_log(int ctarget, ktime_t enter_time)
 			sh->both_blocked++;
 		if (apidle_ape_blocked)
 			sh->ape_blocked++;
+		if (apidle_uart_blocked)
+			sh->uart_blocked++;
+		if (apidle_modem_blocked)
+			sh->modem_blocked++;
 		if (apidle_time_blocked)
 			sh->time_blocked++;
 		if (apidle_gov_blocked)
@@ -457,6 +467,8 @@ static void state_history_reset(void)
 
 		sh->exit_counter = 0;
 		sh->ape_blocked = 0;
+		sh->uart_blocked = 0;
+		sh->modem_blocked = 0;
 		sh->time_blocked = 0;
 		sh->both_blocked = 0;
 		sh->gov_blocked = 0;
@@ -605,8 +617,11 @@ static void stats_disp_one(struct seq_file *s, struct state_history *sh,
 		   (u32) t_us, (u32)perc);
 
 	if (cstates[i].state == CI_IDLE && verbose)
-		seq_printf(s, ", reg:%d time:%d both:%d gov:%d",
-			   sh->ape_blocked, sh->time_blocked,
+		seq_printf(s,
+			   ", reg:%d modem: %d uart: %d time:%d both:%d gov:%d",
+			   sh->ape_blocked,
+			   sh->modem_blocked, sh->uart_blocked,
+			   sh->time_blocked,
 			   sh->both_blocked, sh->gov_blocked);
 
 	if (sh->states[i].counter && verbose)
diff --git a/drivers/cpuidle/cpuidle-dbx500_dbg.h b/drivers/cpuidle/cpuidle-dbx500_dbg.h
index b8089c478a1..99a23984769 100644
--- a/drivers/cpuidle/cpuidle-dbx500_dbg.h
+++ b/drivers/cpuidle/cpuidle-dbx500_dbg.h
@@ -18,8 +18,12 @@ void ux500_ci_dbg_wake_latency(int ctarget, int sleep_time);
 void ux500_ci_dbg_exit_latency(int ctarget, ktime_t now, ktime_t exit,
 			       ktime_t enter);
 
-void ux500_ci_dbg_register_reason(int idx, bool power_state_req,
-				  u32 sleep_time, u32 max_depth);
+void ux500_ci_dbg_register_reason(int idx,
+				  bool ape,
+				  bool modem,
+				  bool uart,
+				  u32 sleep_time,
+				  u32 max_depth);
 
 bool ux500_ci_dbg_force_ape_on(void);
 int ux500_ci_dbg_deepest_state(void);
@@ -43,8 +47,12 @@ static inline void ux500_ci_dbg_exit_latency(int ctarget,
 static inline void ux500_ci_dbg_wake_latency(int ctarget, int sleep_time) { }
 
 
-static inline void ux500_ci_dbg_register_reason(int idx, bool power_state_req,
-						u32 sleep_time, u32 max_depth) { }
+static inline void ux500_ci_dbg_register_reason(int idx,
+						bool ape,
+						bool modem,
+						bool uart,
+						u32 sleep_time,
+						u32 max_depth) { }
 
 static inline bool ux500_ci_dbg_force_ape_on(void)
 {
diff --git a/drivers/crypto/ux500/hash/hash_alg.h b/drivers/crypto/ux500/hash/hash_alg.h
index 299f0bacc2c..61db5b511b6 100644
--- a/drivers/crypto/ux500/hash/hash_alg.h
+++ b/drivers/crypto/ux500/hash/hash_alg.h
@@ -11,6 +11,7 @@
 #include <linux/bitops.h>
 
 #define HASH_BLOCK_SIZE			64
+#define HASH_DMA_ALIGN_SIZE		4
 
 /* Maximum value of the length's high word */
 #define HASH_HIGH_WORD_MAX_VAL		0xFFFFFFFFUL
@@ -125,6 +126,12 @@
 			0x01, HASH_STR_DCAL_POS,	\
 			HASH_STR_DCAL_MASK)
 
+/* Hardware access method */
+enum hash_mode {
+	HASH_MODE_CPU,
+	HASH_MODE_DMA
+};
+
 /**
  * struct uint64 - Structure to handle 64 bits integers.
  * @high_word:	Most significant bits.
@@ -287,14 +294,36 @@ struct hash_config {
 };
 
 /**
+ * struct hash_dma - Structure used for dma.
+ * @mask:		DMA capabilities bitmap mask.
+ * @complete:		Used to maintain state for a "completion".
+ * @chan_mem2hash:	DMA channel.
+ * @cfg_mem2hash:	DMA channel configuration.
+ * @sg_len:		Scatterlist length.
+ * @sg:			Scatterlist.
+ * @nents:		Number of sg entries.
+ */
+struct hash_dma {
+	dma_cap_mask_t		mask;
+	struct completion	complete;
+	struct dma_chan		*chan_mem2hash;
+	void			*cfg_mem2hash;
+	int			sg_len;
+	struct scatterlist	*sg;
+	int			nents;
+};
+
+/**
  * struct hash_ctx - The context used for hash calculations.
  * @key:	The key used in the operation.
  * @keylen:	The length of the key.
  * @updated:	Indicates if hardware is initialized for new operations.
  * @state:	The state of the current calculations.
  * @config:	The current configuration.
- * @digestsize	The size of current digest.
- * @device	Pointer to the device structure.
+ * @digestsize:	The size of current digest.
+ * @device:	Pointer to the device structure.
+ * @dma_mode:	Used in special cases (workaround), e.g. need to change to
+ *		cpu mode, if not supported/working in dma mode.
  */
 struct hash_ctx {
 	u8			*key;
@@ -304,6 +333,7 @@ struct hash_ctx {
 	struct hash_config	config;
 	int			digestsize;
 	struct hash_device_data	*device;
+	bool			dma_mode;
 };
 
 /**
@@ -318,6 +348,7 @@ struct hash_ctx {
  * @regulator:		Pointer to the device's power control.
  * @clk:		Pointer to the device's clock control.
  * @restore_dev_state:	TRUE = saved state, FALSE = no saved state.
+ * @dma:		Structure used for dma.
  */
 struct hash_device_data {
 	struct hash_register __iomem	*base;
@@ -330,6 +361,7 @@ struct hash_device_data {
 	struct ux500_regulator		*regulator;
 	struct clk			*clk;
 	bool				restore_dev_state;
+	struct hash_dma			dma;
 };
 
 int hash_check_hw(struct hash_device_data *device_data);
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
index 08a89eeb601..b2a58dccf76 100644
--- a/drivers/crypto/ux500/hash/hash_core.c
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -22,6 +22,7 @@
 #include <linux/crypto.h>
 
 #include <linux/regulator/dbx500-prcmu.h>
+#include <linux/dmaengine.h>
 #include <linux/bitops.h>
 
 #include <crypto/internal/hash.h>
@@ -29,12 +30,17 @@
 #include <crypto/scatterwalk.h>
 #include <crypto/algapi.h>
 
+#include <mach/crypto-ux500.h>
 #include <mach/hardware.h>
 
 #include "hash_alg.h"
 
 #define DEV_DBG_NAME "hashX hashX:"
 
+static int hash_mode;
+module_param(hash_mode, int, 0);
+MODULE_PARM_DESC(hash_mode, "CPU or DMA mode. CPU = 0 (default), DMA = 1");
+
 /**
  * Pre-calculated empty message digests.
  */
@@ -113,6 +119,101 @@ static void release_hash_device(struct hash_device_data *device_data)
 	up(&driver_data.device_allocation);
 }
 
+static void hash_dma_setup_channel(struct hash_device_data *device_data,
+				struct device *dev)
+{
+	struct hash_platform_data *platform_data = dev->platform_data;
+	dma_cap_zero(device_data->dma.mask);
+	dma_cap_set(DMA_SLAVE, device_data->dma.mask);
+
+	device_data->dma.cfg_mem2hash = platform_data->mem_to_engine;
+	device_data->dma.chan_mem2hash =
+		dma_request_channel(device_data->dma.mask,
+				platform_data->dma_filter,
+				device_data->dma.cfg_mem2hash);
+
+	init_completion(&device_data->dma.complete);
+}
+
+static void hash_dma_callback(void *data)
+{
+	struct hash_ctx *ctx = (struct hash_ctx *) data;
+
+	complete(&ctx->device->dma.complete);
+}
+
+static int hash_set_dma_transfer(struct hash_ctx *ctx, struct scatterlist *sg,
+		int len, enum dma_data_direction direction)
+{
+	struct dma_async_tx_descriptor *desc = NULL;
+	struct dma_chan *channel = NULL;
+	dma_cookie_t cookie;
+
+	if (direction != DMA_TO_DEVICE) {
+		dev_err(ctx->device->dev, "[%s] Invalid DMA direction",
+				__func__);
+		return -EFAULT;
+	}
+
+	sg->length = ALIGN(sg->length, HASH_DMA_ALIGN_SIZE);
+
+	channel = ctx->device->dma.chan_mem2hash;
+	ctx->device->dma.sg = sg;
+	ctx->device->dma.sg_len = dma_map_sg(channel->device->dev,
+			ctx->device->dma.sg, ctx->device->dma.nents,
+			direction);
+
+	if (!ctx->device->dma.sg_len) {
+		dev_err(ctx->device->dev,
+				"[%s]: Could not map the sg list (TO_DEVICE)",
+				__func__);
+		return -EFAULT;
+	}
+
+	dev_dbg(ctx->device->dev, "[%s]: Setting up DMA for buffer "
+			"(TO_DEVICE)", __func__);
+	desc = channel->device->device_prep_slave_sg(channel,
+			ctx->device->dma.sg, ctx->device->dma.sg_len,
+			direction, DMA_CTRL_ACK | DMA_PREP_INTERRUPT);
+	if (!desc) {
+		dev_err(ctx->device->dev,
+			"[%s]: device_prep_slave_sg() failed!", __func__);
+		return -EFAULT;
+	}
+
+	desc->callback = hash_dma_callback;
+	desc->callback_param = ctx;
+
+	cookie = desc->tx_submit(desc);
+	dma_async_issue_pending(channel);
+
+	return 0;
+}
+
+static void hash_dma_done(struct hash_ctx *ctx)
+{
+	struct dma_chan *chan;
+
+	chan = ctx->device->dma.chan_mem2hash;
+	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dma_unmap_sg(chan->device->dev, ctx->device->dma.sg,
+			ctx->device->dma.sg_len, DMA_TO_DEVICE);
+
+}
+
+static int hash_dma_write(struct hash_ctx *ctx,
+		struct scatterlist *sg, int len)
+{
+	int error = hash_set_dma_transfer(ctx, sg, len, DMA_TO_DEVICE);
+	if (error) {
+		dev_dbg(ctx->device->dev, "[%s]: hash_set_dma_transfer() "
+			"failed", __func__);
+		return error;
+	}
+
+	return len;
+}
+
 /**
  * get_empty_message_digest - Returns a pre-calculated digest for
  * the empty message.
@@ -197,8 +298,6 @@ static int hash_disable_power(
 	int ret = 0;
 	struct device *dev = device_data->dev;
 
-	dev_dbg(dev, "[%s]", __func__);
-
 	spin_lock(&device_data->power_state_lock);
 	if (!device_data->power_state)
 		goto out;
@@ -236,7 +335,6 @@ static int hash_enable_power(
 {
 	int ret = 0;
 	struct device *dev = device_data->dev;
-	dev_dbg(dev, "[%s]", __func__);
 
 	spin_lock(&device_data->power_state_lock);
 	if (!device_data->power_state) {
@@ -287,8 +385,6 @@ static int hash_get_device_data(struct hash_ctx *ctx,
 	struct klist_node	*device_node;
 	struct hash_device_data *local_device_data = NULL;
 
-	pr_debug(DEV_DBG_NAME " [%s]", __func__);
-
 	/* Wait until a device is available */
 	ret = down_interruptible(&driver_data.device_allocation);
 	if (ret)
@@ -390,8 +486,6 @@ static int init_hash_hw(struct hash_device_data *device_data,
 {
 	int ret = 0;
 
-	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32)ctx);
-
 	ret = hash_setconfiguration(device_data, &ctx->config);
 	if (ret) {
 		dev_err(device_data->dev, "[%s] hash_setconfiguration() "
@@ -408,6 +502,61 @@ static int init_hash_hw(struct hash_device_data *device_data,
 }
 
 /**
+ * hash_get_nents - Return number of entries (nents) in scatterlist (sg).
+ *
+ * @sg:		Scatterlist.
+ * @size:	Size in bytes.
+ * @aligned:	True if sg data aligned to work in DMA mode.
+ *
+ * Reentrancy: Non Re-entrant
+ */
+static int hash_get_nents(struct scatterlist *sg, int size, bool *aligned)
+{
+	int nents = 0;
+	bool aligned_data = true;
+
+	while (size > 0 && sg) {
+		nents++;
+		size -= sg->length;
+
+		/* hash_set_dma_transfer will align last nent */
+		if (aligned && !IS_ALIGNED(sg->offset, HASH_DMA_ALIGN_SIZE) ||
+			(!IS_ALIGNED(sg->length, HASH_DMA_ALIGN_SIZE) &&
+			 size > 0))
+			aligned_data = false;
+
+		sg = sg_next(sg);
+	}
+
+	if (aligned)
+		*aligned = aligned_data;
+
+	if (size != 0)
+		return -EFAULT;
+
+	return nents;
+}
+
+/**
+ * hash_dma_valid_data - checks for dma valid sg data.
+ * @sg:		Scatterlist.
+ * @datasize:	Datasize in bytes.
+ *
+ * NOTE! This function checks for dma valid sg data, since dma
+ * only accept datasizes of even wordsize.
+ */
+static bool hash_dma_valid_data(struct scatterlist *sg, int datasize)
+{
+	bool aligned;
+
+	/* Need to include at least one nent, else error */
+	if (hash_get_nents(sg, datasize, &aligned) < 1)
+		return false;
+
+	return aligned;
+}
+
+/**
  * hash_init - Common hash init function for SHA1/SHA2 (SHA256).
  * @req: The hash request for the job.
  *
@@ -418,13 +567,39 @@ static int hash_init(struct ahash_request *req)
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-	pr_debug(DEV_DBG_NAME " [%s] data size: %d", __func__, req->nbytes);
-
 	if (!ctx->key)
 		ctx->keylen = 0;
 
 	memset(&ctx->state, 0, sizeof(struct hash_state));
 	ctx->updated = 0;
+	if (hash_mode == HASH_MODE_DMA) {
+		if ((ctx->config.oper_mode == HASH_OPER_MODE_HMAC) &&
+				cpu_is_u5500()) {
+			pr_debug(DEV_DBG_NAME " [%s] HMAC and DMA not working "
+					"on u5500, directing to CPU mode.",
+					__func__);
+			ctx->dma_mode = false; /* Don't use DMA in this case */
+			goto out;
+		}
+
+		if (req->nbytes < HASH_DMA_ALIGN_SIZE) {
+			ctx->dma_mode = false; /* Don't use DMA in this case */
+
+			pr_debug(DEV_DBG_NAME " [%s] DMA mode, but direct "
+					"to CPU mode for data size < %d",
+					__func__, HASH_DMA_ALIGN_SIZE);
+		} else {
+			if (hash_dma_valid_data(req->src, req->nbytes)) {
+				ctx->dma_mode = true;
+			} else {
+				ctx->dma_mode = false;
+				pr_debug(DEV_DBG_NAME " [%s] DMA mode, but "
+						"direct to CPU mode for "
+						"non-aligned data", __func__);
+			}
+		}
+	}
+out:
 	return 0;
 }
 
@@ -474,9 +649,6 @@ static void hash_processblock(
 static void hash_messagepad(struct hash_device_data *device_data,
 		const u32 *message, u8 index_bytes)
 {
-	dev_dbg(device_data->dev, "[%s] (bytes in final msg=%d))",
-			__func__, index_bytes);
-
 	/*
 	 * Clear hash str register, only clear NBLW
 	 * since DCAL will be reset by hardware.
@@ -561,7 +733,6 @@ int hash_setconfiguration(struct hash_device_data *device_data,
 		struct hash_config *config)
 {
 	int ret = 0;
-	dev_dbg(device_data->dev, "[%s] ", __func__);
 
 	if (config->algorithm != HASH_ALGO_SHA1 &&
 	    config->algorithm != HASH_ALGO_SHA256)
@@ -574,13 +745,6 @@ int hash_setconfiguration(struct hash_device_data *device_data,
 	HASH_SET_DATA_FORMAT(config->data_format);
 
 	/*
-	 * Empty message bit. This bit is needed when the hash input data
-	 * contain the empty message. Always set in current impl. but with
-	 * no impact on data different than empty message.
-	 */
-	HASH_SET_BITS(&device_data->base->cr, HASH_CR_EMPTYMSG_MASK);
-
-	/*
 	 * ALGO bit. Set to 0b1 for SHA-1 and 0b0 for SHA-256
 	 */
 	switch (config->algorithm) {
@@ -652,7 +816,6 @@ void hash_begin(struct hash_device_data *device_data, struct hash_ctx *ctx)
 {
 	/* HW and SW initializations */
 	/* Note: there is no need to initialize buffer and digest members */
-	dev_dbg(device_data->dev, "[%s] ", __func__);
 
 	while (device_data->base->str & HASH_STR_DCAL_MASK)
 		cpu_relax();
@@ -688,6 +851,7 @@ int hash_process_data(
 			msg_length = 0;
 		} else {
 			if (ctx->updated) {
+
 				ret = hash_resume_state(device_data,
 						&ctx->state);
 				if (ret) {
@@ -696,7 +860,6 @@ int hash_process_data(
 							" failed!", __func__);
 					goto out;
 				}
-
 			} else {
 				ret = init_hash_hw(device_data, ctx);
 				if (ret) {
@@ -732,6 +895,7 @@ int hash_process_data(
 			}
 			hash_incrementlength(ctx, HASH_BLOCK_SIZE);
 			data_buffer += (HASH_BLOCK_SIZE - *index);
+
 			msg_length -= (HASH_BLOCK_SIZE - *index);
 			*index = 0;
 
@@ -751,6 +915,236 @@ out:
 }
 
 /**
+ * hash_dma_final - The hash dma final function for SHA1/SHA256.
+ * @req:	The hash request for the job.
+ */
+static int hash_dma_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_device_data *device_data;
+	u8 digest[SHA256_DIGEST_SIZE];
+	int bytes_written = 0;
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32) ctx);
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+				"hash_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	if (ctx->updated) {
+		ret = hash_resume_state(device_data, &ctx->state);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_resume_state() "
+					"failed!", __func__);
+			goto out_power;
+		}
+
+	}
+
+	if (!ctx->updated) {
+		ret = hash_setconfiguration(device_data, &ctx->config);
+		if (ret) {
+			dev_err(device_data->dev, "[%s] "
+					"hash_setconfiguration() failed!",
+					__func__);
+			goto out_power;
+		}
+
+		/* Enable DMA input */
+		if (hash_mode != HASH_MODE_DMA || !ctx->dma_mode) {
+			HASH_CLEAR_BITS(&device_data->base->cr,
+					HASH_CR_DMAE_MASK);
+		} else {
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_DMAE_MASK);
+			HASH_SET_BITS(&device_data->base->cr,
+					HASH_CR_PRIVN_MASK);
+		}
+
+		HASH_INITIALIZE;
+
+		if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC)
+			hash_hw_write_key(device_data, ctx->key, ctx->keylen);
+
+		/* Number of bits in last word = (nbytes * 8) % 32 */
+		HASH_SET_NBLW((req->nbytes * 8) % 32);
+		ctx->updated = 1;
+	}
+
+	/* Store the nents in the dma struct. */
+	ctx->device->dma.nents = hash_get_nents(req->src, req->nbytes, NULL);
+	if (!ctx->device->dma.nents) {
+		dev_err(device_data->dev, "[%s] "
+				"ctx->device->dma.nents = 0", __func__);
+		goto out_power;
+	}
+
+	bytes_written = hash_dma_write(ctx, req->src, req->nbytes);
+	if (bytes_written != req->nbytes) {
+		dev_err(device_data->dev, "[%s] "
+				"hash_dma_write() failed!", __func__);
+		goto out_power;
+	}
+
+	wait_for_completion(&ctx->device->dma.complete);
+	hash_dma_done(ctx);
+
+	while (device_data->base->str & HASH_STR_DCAL_MASK)
+		cpu_relax();
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC && ctx->key) {
+		unsigned int keylen = ctx->keylen;
+		u8 *key = ctx->key;
+
+		dev_dbg(device_data->dev, "[%s] keylen: %d", __func__,
+				ctx->keylen);
+		hash_hw_write_key(device_data, key, keylen);
+	}
+
+	hash_get_digest(device_data, digest, ctx->config.algorithm);
+	memcpy(req->result, digest, ctx->digestsize);
+
+out_power:
+	/* Disable power (and clock) */
+	if (hash_disable_power(device_data, false))
+		dev_err(device_data->dev, "[%s] hash_disable_power() failed!",
+				__func__);
+
+out:
+	release_hash_device(device_data);
+
+	/**
+	 * Allocated in setkey, and only used in HMAC.
+	 */
+	kfree(ctx->key);
+
+	return ret;
+}
+
+/**
+ * hash_hw_final - The final hash calculation function
+ * @req:	The hash request for the job.
+ */
+int hash_hw_final(struct ahash_request *req)
+{
+	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
+	struct hash_device_data *device_data;
+	u8 digest[SHA256_DIGEST_SIZE];
+
+	ret = hash_get_device_data(ctx, &device_data);
+	if (ret)
+		return ret;
+
+	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32) ctx);
+
+	/* Enable device power (and clock) */
+	ret = hash_enable_power(device_data, false);
+	if (ret) {
+		dev_err(device_data->dev, "[%s]: "
+				"hash_enable_power() failed!", __func__);
+		goto out;
+	}
+
+	if (ctx->updated) {
+		ret = hash_resume_state(device_data, &ctx->state);
+
+		if (ret) {
+			dev_err(device_data->dev, "[%s] hash_resume_state() "
+					"failed!", __func__);
+			goto out_power;
+		}
+	} else if (req->nbytes == 0 && ctx->keylen == 0) {
+		u8 zero_hash[SHA256_DIGEST_SIZE];
+		u32 zero_hash_size = 0;
+		bool zero_digest = false;
+		/**
+		 * Use a pre-calculated empty message digest
+		 * (workaround since hw return zeroes, hw bug!?)
+		 */
+		ret = get_empty_message_digest(device_data, &zero_hash[0],
+				&zero_hash_size, &zero_digest);
+		if (!ret && likely(zero_hash_size == ctx->digestsize) &&
+				zero_digest) {
+			memcpy(req->result, &zero_hash[0], ctx->digestsize);
+			goto out_power;
+		} else if (!ret && !zero_digest) {
+			dev_dbg(device_data->dev, "[%s] HMAC zero msg with "
+					"key, continue...", __func__);
+		} else {
+			dev_err(device_data->dev, "[%s] ret=%d, or wrong "
+					"digest size? %s", __func__, ret,
+					(zero_hash_size == ctx->digestsize) ?
+					"true" : "false");
+			/* Return error */
+			goto out_power;
+		}
+	} else if (req->nbytes == 0 && ctx->keylen > 0) {
+		dev_err(device_data->dev, "[%s] Empty message with "
+				"keylength > 0, NOT supported.", __func__);
+		goto out_power;
+	}
+
+	if (!ctx->updated) {
+		ret = init_hash_hw(device_data, ctx);
+		if (ret) {
+			dev_err(device_data->dev, "[%s] init_hash_hw() "
+					"failed!", __func__);
+			goto out_power;
+		}
+	}
+
+	if (ctx->state.index) {
+		hash_messagepad(device_data, ctx->state.buffer,
+				ctx->state.index);
+	} else {
+		HASH_SET_DCAL;
+		while (device_data->base->str & HASH_STR_DCAL_MASK)
+			cpu_relax();
+	}
+
+	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC && ctx->key) {
+		unsigned int keylen = ctx->keylen;
+		u8 *key = ctx->key;
+
+		dev_dbg(device_data->dev, "[%s] keylen: %d", __func__,
+				ctx->keylen);
+		hash_hw_write_key(device_data, key, keylen);
+	}
+
+	hash_get_digest(device_data, digest, ctx->config.algorithm);
+	memcpy(req->result, digest, ctx->digestsize);
+
+out_power:
+	/* Disable power (and clock) */
+	if (hash_disable_power(device_data, false))
+		dev_err(device_data->dev, "[%s] hash_disable_power() failed!",
+				__func__);
+
+out:
+	release_hash_device(device_data);
+
+	/**
+	 * Allocated in setkey, and only used in HMAC.
+	 */
+	kfree(ctx->key);
+
+	return ret;
+}
+
+/**
  * hash_hw_update - Updates current HASH computation hashing another part of
  *                  the message.
  * @req:	Byte array containing the message to be hashed (caller
@@ -770,8 +1164,6 @@ int hash_hw_update(struct ahash_request *req)
 	struct crypto_hash_walk walk;
 	int msg_length = crypto_hash_walk_first(req, &walk);
 
-	pr_debug(DEV_DBG_NAME " [%s] datalength: %d", __func__, msg_length);
-
 	/* Empty message ("") is correct indata */
 	if (msg_length == 0)
 		return ret;
@@ -818,9 +1210,9 @@ int hash_hw_update(struct ahash_request *req)
 	}
 
 	ctx->state.index = index;
-
 	dev_dbg(device_data->dev, "[%s] indata length=%d, "
-		"bin=%d))", __func__, ctx->state.index,	ctx->state.bit_index);
+		"bin=%d))", __func__, ctx->state.index, ctx->state.bit_index);
+
 out_power:
 	/* Disable power (and clock) */
 	if (hash_disable_power(device_data, false))
@@ -846,9 +1238,6 @@ int hash_resume_state(struct hash_device_data *device_data,
 	s32 count;
 	int hash_mode = HASH_OPER_MODE_HASH;
 
-	dev_dbg(device_data->dev, "[%s] (state(0x%x)))",
-			__func__, (u32) device_state);
-
 	if (NULL == device_state) {
 		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
 				__func__);
@@ -909,9 +1298,6 @@ int hash_save_state(struct hash_device_data *device_data,
 	u32 count;
 	int hash_mode = HASH_OPER_MODE_HASH;
 
-	dev_dbg(device_data->dev, "[%s] state(0x%x)))",
-		 __func__, (u32) device_state);
-
 	if (NULL == device_state) {
 		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
 				__func__);
@@ -961,8 +1347,6 @@ int hash_check_hw(struct hash_device_data *device_data)
 {
 	int ret = 0;
 
-	dev_dbg(device_data->dev, "[%s] ", __func__);
-
 	if (NULL == device_data) {
 		ret = -EPERM;
 		dev_err(device_data->dev, "[%s] HASH_INVALID_PARAMETER!",
@@ -1041,17 +1425,18 @@ void hash_get_digest(struct hash_device_data *device_data,
 static int ahash_update(struct ahash_request *req)
 {
 	int ret = 0;
+	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
+	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-	pr_debug(DEV_DBG_NAME " [%s] ", __func__);
+	if (hash_mode != HASH_MODE_DMA || !ctx->dma_mode)
+		ret = hash_hw_update(req);
+	/* Skip update for DMA, all data will be passed to DMA in final */
 
-	ret = hash_hw_update(req);
 	if (ret) {
 		pr_err(DEV_DBG_NAME " [%s] hash_hw_update() failed!",
 				__func__);
-		goto out;
 	}
 
-out:
 	return ret;
 }
 
@@ -1064,103 +1449,18 @@ static int ahash_final(struct ahash_request *req)
 	int ret = 0;
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
-	struct hash_device_data *device_data;
-	u8 digest[SHA256_DIGEST_SIZE];
-
-	pr_debug(DEV_DBG_NAME " [%s] ", __func__);
 
-	ret = hash_get_device_data(ctx, &device_data);
-	if (ret)
-		return ret;
+	pr_debug(DEV_DBG_NAME " [%s] data size: %d", __func__, req->nbytes);
 
-	dev_dbg(device_data->dev, "[%s] (ctx=0x%x)!", __func__, (u32) ctx);
+	if ((hash_mode == HASH_MODE_DMA) && ctx->dma_mode)
+		ret = hash_dma_final(req);
+	else
+		ret = hash_hw_final(req);
 
-	/* Enable device power (and clock) */
-	ret = hash_enable_power(device_data, false);
 	if (ret) {
-		dev_err(device_data->dev, "[%s]: "
-				"hash_enable_power() failed!", __func__);
-		goto out;
-	}
-
-	if (ctx->updated) {
-		ret = hash_resume_state(device_data, &ctx->state);
-
-		if (ret) {
-			dev_err(device_data->dev, "[%s] hash_resume_state() "
-					"failed!", __func__);
-			goto out_power;
-		}
-	} else if (req->nbytes == 0 && ctx->keylen == 0) {
-		u8 zero_hash[SHA256_DIGEST_SIZE];
-		u32 zero_hash_size = 0;
-		bool zero_digest = false;
-		/**
-		 * Use a pre-calculated empty message digest
-		 * (workaround since hw return zeroes, hw bug!?)
-		 */
-		ret = get_empty_message_digest(device_data, &zero_hash[0],
-				&zero_hash_size, &zero_digest);
-		if (!ret && likely(zero_hash_size == ctx->digestsize) &&
-				zero_digest) {
-			memcpy(req->result, &zero_hash[0], ctx->digestsize);
-			goto out_power;
-		} else if (!ret && !zero_digest) {
-			dev_dbg(device_data->dev, "[%s] HMAC zero msg with "
-					"key, continue...", __func__);
-		} else {
-			dev_err(device_data->dev, "[%s] ret=%d, or wrong "
-					"digest size? %s", __func__, ret,
-					(zero_hash_size == ctx->digestsize) ?
-					"true" : "false");
-			/* Return error */
-			goto out_power;
-		}
-	}
-
-	if (!ctx->updated) {
-		ret = init_hash_hw(device_data, ctx);
-		if (ret) {
-			dev_err(device_data->dev, "[%s] init_hash_hw() "
-					"failed!", __func__);
-			goto out_power;
-		}
-	}
-
-	if (ctx->state.index) {
-		hash_messagepad(device_data, ctx->state.buffer,
-				ctx->state.index);
-	} else {
-		HASH_SET_DCAL;
-		while (device_data->base->str & HASH_STR_DCAL_MASK)
-			cpu_relax();
-	}
-
-	if (ctx->config.oper_mode == HASH_OPER_MODE_HMAC && ctx->key) {
-		unsigned int keylen = ctx->keylen;
-		u8 *key = ctx->key;
-
-		dev_dbg(device_data->dev, "[%s] keylen: %d", __func__,
-				ctx->keylen);
-		hash_hw_write_key(device_data, key, keylen);
-	}
-
-	hash_get_digest(device_data, digest, ctx->config.algorithm);
-	memcpy(req->result, digest, ctx->digestsize);
-
-out_power:
-	/* Disable power (and clock) */
-	if (hash_disable_power(device_data, false))
-		dev_err(device_data->dev, "[%s] hash_disable_power() failed!",
+		pr_err(DEV_DBG_NAME " [%s] hash_hw/dma_final() failed",
 				__func__);
-
-out:
-	release_hash_device(device_data);
-
-	/**
-	 * Allocated in setkey, and only used in HMAC.
-	 */
-	kfree(ctx->key);
+	}
 
 	return ret;
 }
@@ -1171,8 +1471,6 @@ static int hash_setkey(struct crypto_ahash *tfm,
 	int ret = 0;
 	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-	pr_debug(DEV_DBG_NAME " [%s] keylen: %d", __func__, keylen);
-
 	/**
 	 * Freed in final.
 	 */
@@ -1194,8 +1492,6 @@ static int ahash_sha1_init(struct ahash_request *req)
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-	pr_debug(DEV_DBG_NAME " [%s]: (ctx=0x%x)!", __func__, (u32) ctx);
-
 	ctx->config.data_format = HASH_DATA_8_BITS;
 	ctx->config.algorithm = HASH_ALGO_SHA1;
 	ctx->config.oper_mode = HASH_OPER_MODE_HASH;
@@ -1209,8 +1505,6 @@ static int ahash_sha256_init(struct ahash_request *req)
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-	pr_debug(DEV_DBG_NAME " [%s]: (ctx=0x%x)!", __func__, (u32) ctx);
-
 	ctx->config.data_format = HASH_DATA_8_BITS;
 	ctx->config.algorithm = HASH_ALGO_SHA256;
 	ctx->config.oper_mode = HASH_OPER_MODE_HASH;
@@ -1223,8 +1517,6 @@ static int ahash_sha1_digest(struct ahash_request *req)
 {
 	int ret2, ret1;
 
-	pr_debug(DEV_DBG_NAME " [%s]", __func__);
-
 	ret1 = ahash_sha1_init(req);
 	if (ret1)
 		goto out;
@@ -1240,8 +1532,6 @@ static int ahash_sha256_digest(struct ahash_request *req)
 {
 	int ret2, ret1;
 
-	pr_debug(DEV_DBG_NAME " [%s]", __func__);
-
 	ret1 = ahash_sha256_init(req);
 	if (ret1)
 		goto out;
@@ -1258,8 +1548,6 @@ static int hmac_sha1_init(struct ahash_request *req)
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-	pr_debug(DEV_DBG_NAME " [%s]: (ctx=0x%x)!", __func__, (u32) ctx);
-
 	ctx->config.data_format	= HASH_DATA_8_BITS;
 	ctx->config.algorithm	= HASH_ALGO_SHA1;
 	ctx->config.oper_mode	= HASH_OPER_MODE_HMAC;
@@ -1273,8 +1561,6 @@ static int hmac_sha256_init(struct ahash_request *req)
 	struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
 	struct hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-	pr_debug(DEV_DBG_NAME " [%s]: (ctx=0x%x)!", __func__, (u32) ctx);
-
 	ctx->config.data_format	= HASH_DATA_8_BITS;
 	ctx->config.algorithm	= HASH_ALGO_SHA256;
 	ctx->config.oper_mode	= HASH_OPER_MODE_HMAC;
@@ -1287,8 +1573,6 @@ static int hmac_sha1_digest(struct ahash_request *req)
 {
 	int ret2, ret1;
 
-	pr_debug(DEV_DBG_NAME " [%s]", __func__);
-
 	ret1 = hmac_sha1_init(req);
 	if (ret1)
 		goto out;
@@ -1304,8 +1588,6 @@ static int hmac_sha256_digest(struct ahash_request *req)
 {
 	int ret2, ret1;
 
-	pr_debug(DEV_DBG_NAME " [%s]", __func__);
-
 	ret1 = hmac_sha256_init(req);
 	if (ret1)
 		goto out;
@@ -1320,16 +1602,12 @@ out:
 static int hmac_sha1_setkey(struct crypto_ahash *tfm,
 		const u8 *key, unsigned int keylen)
 {
-	pr_debug(DEV_DBG_NAME " [%s]", __func__);
-
 	return hash_setkey(tfm, key, keylen, HASH_ALGO_SHA1);
 }
 
 static int hmac_sha256_setkey(struct crypto_ahash *tfm,
 		const u8 *key, unsigned int keylen)
 {
-	pr_debug(DEV_DBG_NAME " [%s]", __func__);
-
 	return hash_setkey(tfm, key, keylen, HASH_ALGO_SHA256);
 }
 
@@ -1425,8 +1703,6 @@ static int ahash_algs_register_all(struct hash_device_data *device_data)
 	int i;
 	int count;
 
-	dev_dbg(device_data->dev, "[%s]", __func__);
-
 	for (i = 0; i < ARRAY_SIZE(ux500_ahash_algs); i++) {
 		ret = crypto_register_ahash(ux500_ahash_algs[i]);
 		if (ret) {
@@ -1451,8 +1727,6 @@ static void ahash_algs_unregister_all(struct hash_device_data *device_data)
 {
 	int i;
 
-	dev_dbg(device_data->dev, "[%s]", __func__);
-
 	for (i = 0; i < ARRAY_SIZE(ux500_ahash_algs); i++)
 		crypto_unregister_ahash(ux500_ahash_algs[i]);
 }
@@ -1468,7 +1742,6 @@ static int ux500_hash_probe(struct platform_device *pdev)
 	struct hash_device_data *device_data;
 	struct device		*dev = &pdev->dev;
 
-	dev_dbg(dev, "[%s] (pdev=0x%x)", __func__, (u32) pdev);
 	device_data = kzalloc(sizeof(struct hash_device_data), GFP_ATOMIC);
 	if (!device_data) {
 		dev_dbg(dev, "[%s] kzalloc() failed!", __func__);
@@ -1505,7 +1778,6 @@ static int ux500_hash_probe(struct platform_device *pdev)
 
 	/* Enable power for HASH1 hardware block */
 	device_data->regulator = ux500_regulator_get(dev);
-
 	if (IS_ERR(device_data->regulator)) {
 		dev_err(dev, "[%s] regulator_get() failed!", __func__);
 		ret = PTR_ERR(device_data->regulator);
@@ -1534,6 +1806,9 @@ static int ux500_hash_probe(struct platform_device *pdev)
 		goto out_power;
 	}
 
+	if (hash_mode == HASH_MODE_DMA)
+		hash_dma_setup_channel(device_data, dev);
+
 	platform_set_drvdata(pdev, device_data);
 
 	/* Put the new device into the device list... */
@@ -1585,8 +1860,6 @@ static int ux500_hash_remove(struct platform_device *pdev)
 	struct hash_device_data *device_data;
 	struct device		*dev = &pdev->dev;
 
-	dev_dbg(dev, "[%s] (pdev=0x%x)", __func__, (u32) pdev);
-
 	device_data = platform_get_drvdata(pdev);
 	if (!device_data) {
 		dev_err(dev, "[%s]: platform_get_drvdata() failed!",
@@ -1646,8 +1919,6 @@ static void ux500_hash_shutdown(struct platform_device *pdev)
 	struct resource *res = NULL;
 	struct hash_device_data *device_data;
 
-	dev_dbg(&pdev->dev, "[%s]", __func__);
-
 	device_data = platform_get_drvdata(pdev);
 	if (!device_data) {
 		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
@@ -1701,8 +1972,6 @@ static int ux500_hash_suspend(struct platform_device *pdev, pm_message_t state)
 	struct hash_device_data *device_data;
 	struct hash_ctx *temp_ctx = NULL;
 
-	dev_dbg(&pdev->dev, "[%s]", __func__);
-
 	device_data = platform_get_drvdata(pdev);
 	if (!device_data) {
 		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
@@ -1740,8 +2009,6 @@ static int ux500_hash_resume(struct platform_device *pdev)
 	struct hash_device_data *device_data;
 	struct hash_ctx *temp_ctx = NULL;
 
-	dev_dbg(&pdev->dev, "[%s]", __func__);
-
 	device_data = platform_get_drvdata(pdev);
 	if (!device_data) {
 		dev_err(&pdev->dev, "[%s] platform_get_drvdata() failed!",
@@ -1783,7 +2050,6 @@ static struct platform_driver hash_driver = {
  */
 static int __init ux500_hash_mod_init(void)
 {
-	pr_debug(DEV_DBG_NAME " [%s] is called!", __func__);
 	klist_init(&driver_data.device_list, NULL, NULL);
 	/* Initialize the semaphore to 0 devices (locked state) */
 	sema_init(&driver_data.device_allocation, 0);
@@ -1796,8 +2062,6 @@ static int __init ux500_hash_mod_init(void)
  */
 static void __exit ux500_hash_mod_fini(void)
 {
-	pr_debug(DEV_DBG_NAME " [%s] is called!", __func__);
-
 	platform_driver_unregister(&hash_driver);
 	return;
 }
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index eb241159cae..5f33a91533b 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -209,6 +209,7 @@ struct d40_lcla_pool {
  * @allocated_dst: Same as for src but is dst.
  * allocated_dst and allocated_src uses the D40_ALLOC* defines as well as
  * event line number.
+ * @use_soft_lli: To mark if the linked lists of channel are managed by SW.
  */
 struct d40_phy_res {
 	spinlock_t lock;
@@ -216,6 +217,7 @@ struct d40_phy_res {
 	int	   num;
 	u32	   allocated_src;
 	u32	   allocated_dst;
+	bool	   use_soft_lli;
 };
 
 struct d40_base;
@@ -244,7 +246,6 @@ struct d40_base;
  * @dma_cfg: The client configuration of this dma channel.
  * @configured: whether the dma_cfg configuration is valid
  * @base: Pointer to the device instance struct.
- * @cdesc: Cyclic descriptor
  * @src_def_cfg: Default cfg register setting for src.
  * @dst_def_cfg: Default cfg register setting for dst.
  * @log_def: Default logical channel settings.
@@ -275,7 +276,6 @@ struct d40_chan {
 	struct stedma40_chan_cfg	 dma_cfg;
 	bool				 configured;
 	struct d40_base			*base;
-	struct stedma40_cyclic_desc	*cdesc;
 	/* Default register configurations */
 	u32				 src_def_cfg;
 	u32				 dst_def_cfg;
@@ -634,7 +634,16 @@ static int d40_desc_log_lli_to_lcxa(struct d40_chan *d40c,
 	if ((d40d->lli_len - d40d->lli_current) > 1 ||
 	     d40d->cyclic || !use_lcpa) {
 
-		curr_lcla = d40_lcla_alloc_one(d40c, d40d);
+		/*
+		 * If the channel is expected to use only soft_lli don't
+		 * allocate a lcla. This is to avoid a HW issue that exists
+		 * in some controller during a peripheral to memory transfer
+		 * that uses linked lists.
+		 */
+		if (!(d40c->phy_chan->use_soft_lli &&
+			d40c->dma_cfg.dir == STEDMA40_PERIPH_TO_MEM))
+			curr_lcla = d40_lcla_alloc_one(d40c, d40d);
+
 		first_lcla = curr_lcla;
 	}
 
@@ -1339,8 +1348,7 @@ static dma_cookie_t d40_tx_submit(struct dma_async_tx_descriptor *tx)
 
 	d40d->txd.cookie = d40c->chan.cookie;
 
-	if (!d40c->cdesc)
-		d40_desc_queue(d40c, d40d);
+	d40_desc_queue(d40c, d40d);
 
 	spin_unlock_irqrestore(&d40c->lock, flags);
 
@@ -1391,12 +1399,6 @@ static void dma_tc_handle(struct d40_chan *d40c)
 	struct d40_desc *d40d;
 	bool islastactive;
 
-	if (d40c->cdesc) {
-		d40c->pending_tx++;
-		tasklet_schedule(&d40c->tasklet);
-		return;
-	}
-
 	/* Get first active entry from list */
 redo:
 	d40d = d40_first_active_get(d40c);
@@ -1404,6 +1406,12 @@ redo:
 	if (d40d == NULL)
 		return;
 
+	if (d40d->cyclic) {
+		d40c->pending_tx++;
+		tasklet_schedule(&d40c->tasklet);
+		return;
+	}
+
 	d40_lcla_free_all(d40c, d40d);
 
 	if (d40d->lli_current < d40d->lli_len) {
@@ -1450,16 +1458,17 @@ static void dma_tasklet(unsigned long data)
 
 	spin_lock_irqsave(&d40c->lock, flags);
 
-	if (d40c->cdesc)
-		d40d = d40c->cdesc->d40d;
-	else {
-		/* Get first active entry from list */
-		d40d = d40_first_done(d40c);
-		if (d40d == NULL)
+	/* Get first entry from the done list */
+	d40d = d40_first_done(d40c);
+	if (d40d == NULL) {
+		/* Check if we have reached here for cyclic job */
+		d40d = d40_first_active_get(d40c);
+		if (!d40d->cyclic)
 			goto err;
+	}
 
+	if (!d40d->cyclic)
 		d40c->completed = d40d->txd.cookie;
-	}
 
 	/*
 	 * If terminating a channel pending_tx is set to zero.
@@ -1472,18 +1481,10 @@ static void dma_tasklet(unsigned long data)
 
 	/* Callback to client */
 
-	if (d40c->cdesc) {
-		callback = d40c->cdesc->period_callback;
-		callback_param = d40c->cdesc->period_callback_param;
-
-		if (!callback) {
-			callback = d40d->txd.callback;
-			callback_param = d40d->txd.callback_param;
-		}
-	} else {
-		callback = d40d->txd.callback;
-		callback_param = d40d->txd.callback_param;
+	callback = d40d->txd.callback;
+	callback_param = d40d->txd.callback_param;
 
+	if (!d40d->cyclic) {
 		if (async_tx_test_ack(&d40d->txd)) {
 			d40_desc_remove(d40d);
 			d40_desc_free(d40c, d40d);
@@ -2140,6 +2141,9 @@ d40_prep_sg(struct dma_chan *dchan, struct scatterlist *sg_src,
 	if (desc == NULL)
 		goto err;
 
+	if (sg_next(&sg_src[sg_len - 1]) == sg_src)
+		desc->cyclic = true;
+
 	if (direction != DMA_NONE) {
 		dma_addr_t dev_addr = d40_get_dev_addr(chan, direction);
 
@@ -2410,11 +2414,6 @@ static void d40_issue_pending(struct dma_chan *chan)
 		return;
 	}
 
-	if (d40c->cdesc) {
-		stedma40_cyclic_start(chan);
-		return;
-	}
-
 	spin_lock_irqsave(&d40c->lock, flags);
 
 	list_splice_tail_init(&d40c->pending_queue, &d40c->queue);
@@ -2447,9 +2446,6 @@ static void d40_terminate_all(struct dma_chan *chan)
 	d40c->busy = false;
 
 	spin_unlock_irqrestore(&d40c->lock, flags);
-
-	if (d40c->cdesc)
-		stedma40_cyclic_free(chan);
 }
 
 static int
@@ -2708,194 +2704,32 @@ dma_addr_t stedma40_get_dst_addr(struct dma_chan *chan)
 }
 EXPORT_SYMBOL(stedma40_get_dst_addr);
 
-int stedma40_cyclic_start(struct dma_chan *chan)
-{
-	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
-	unsigned long flags;
-	int ret = -EINVAL;
-
-	spin_lock_irqsave(&d40c->lock, flags);
-
-	if (!d40c->cdesc)
-		goto out;
-
-	d40_usage_inc(d40c);
-
-	ret = d40_start(d40c);
-	if (!ret)
-		d40c->busy = true;
-	else
-		d40_usage_dec(d40c);
-
-out:
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return ret;
-}
-EXPORT_SYMBOL(stedma40_cyclic_start);
-
-void stedma40_cyclic_stop(struct dma_chan *chan)
-{
-	d40_terminate_all(chan);
-}
-EXPORT_SYMBOL(stedma40_cyclic_stop);
-
-void stedma40_cyclic_free(struct dma_chan *chan)
-{
-	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
-	struct stedma40_cyclic_desc *cdesc;
-	unsigned long flags;
-
-	spin_lock_irqsave(&d40c->lock, flags);
-
-	cdesc = d40c->cdesc;
-	if (!cdesc) {
-		spin_unlock_irqrestore(&d40c->lock, flags);
-		return;
-	}
-
-	d40c->cdesc = NULL;
-	d40_lcla_free_all(d40c, cdesc->d40d);
-
-	spin_unlock_irqrestore(&d40c->lock, flags);
-
-	kfree(cdesc);
-}
-EXPORT_SYMBOL(stedma40_cyclic_free);
-
-struct stedma40_cyclic_desc *
-stedma40_cyclic_prep_sg(struct dma_chan *chan,
-			struct scatterlist *sgl,
-			unsigned int sg_len,
-			enum dma_data_direction direction,
-			unsigned long dma_flags)
-{
-	struct d40_chan *d40c = container_of(chan, struct d40_chan, chan);
-	struct stedma40_cyclic_desc *cdesc;
-	struct d40_desc *d40d;
-	dma_addr_t dev_addr;
-	unsigned long flags;
-	void *mem;
-	int err;
-	dma_addr_t src_dev_addr = 0;
-	dma_addr_t dst_dev_addr = 0;
-
-	mem = kzalloc(sizeof(struct stedma40_cyclic_desc)
-		      + sizeof(struct d40_desc), GFP_ATOMIC);
-	if (!mem)
-		return ERR_PTR(-ENOMEM);
-
-	cdesc = mem;
-	d40d = cdesc->d40d = mem + sizeof(struct stedma40_cyclic_desc);
-
-	spin_lock_irqsave(&d40c->lock, flags);
-
-	if (d40c->phy_chan == NULL) {
-		chan_err(d40c, "Cannot prepare unallocated channel\n");
-		err = -EINVAL;
-		goto out;
-	}
-
-	if (d40c->cdesc || d40c->busy) {
-		chan_err(d40c, "Cannot prepare cyclic job for busy channel\n");
-		err = -EBUSY;
-		goto out;
-	}
-
-	d40d->cyclic = true;
-	d40d->txd.flags = dma_flags;
-	INIT_LIST_HEAD(&d40d->node);
-
-	err = d40_pool_lli_alloc(d40c, d40d, sg_len);
-	if (err < 0) {
-		chan_err(d40c, "Could not allocate lli\n");
-		goto out;
-	}
-
-	d40_usage_inc(d40c);
-
-	dev_addr = d40_get_dev_addr(d40c, direction);
-
-	if (direction == DMA_FROM_DEVICE)
-		src_dev_addr = dev_addr;
-	else if (direction == DMA_TO_DEVICE)
-		dst_dev_addr = dev_addr;
-
-	if (chan_is_logical(d40c))
-		err = d40_prep_sg_log(d40c, d40d, sgl, sgl,
-					sg_len, src_dev_addr, dst_dev_addr);
-	else
-		err = d40_prep_sg_phy(d40c, d40d, sgl, sgl,
-					sg_len, src_dev_addr, dst_dev_addr);
-
-	if (err) {
-		chan_err(d40c,"Failed to prepare %s slave sg job: %d\n",
-			chan_is_logical(d40c) ? "log" : "phy", err);
-		goto out2;
-	}
-
-	d40d->lli_len = sg_len;
-	d40d->lli_current = 0;
-
-	d40_desc_load(d40c, d40d);
-
-	/*
-	 * Couldn't get enough LCLA.  We don't support splitting of cyclic
-	 * jobs.
-	 */
-	if (d40d->lli_current != d40d->lli_len) {
-		chan_err(d40c,"Couldn't prepare cyclic job: not enough LCLA");
-		err = -EBUSY;
-		goto out2;
-	}
-
-	d40c->cdesc = cdesc;
-	d40_usage_dec(d40c);
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return cdesc;
-out2:
-	d40_usage_dec(d40c);
-out:
-	if (d40c->phy_chan)
-		d40_lcla_free_all(d40c, cdesc->d40d);
-	kfree(cdesc);
-	spin_unlock_irqrestore(&d40c->lock, flags);
-	return ERR_PTR(err);
-}
-EXPORT_SYMBOL(stedma40_cyclic_prep_sg);
-
-struct dma_async_tx_descriptor *
-dma40_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)
+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)
 {
-	unsigned int sg_len = buf_len / period_len;
-	struct stedma40_cyclic_desc *cdesc;
+	unsigned int periods = buf_len / period_len;
 	struct dma_async_tx_descriptor *txd;
 	struct scatterlist *sg;
 	int i;
 
-	sg = kzalloc(sizeof(struct scatterlist) * sg_len, GFP_ATOMIC);
-	if (!sg)
-		return ERR_PTR(-ENOMEM);
-
-	sg_init_table(sg, sg_len);
-	for (i = 0; i < sg_len; i++) {
-	        sg_dma_address(&sg[i]) = buf_addr + i * period_len;
-	        sg_dma_len(&sg[i]) = period_len;
+	sg = kcalloc(periods + 1, sizeof(struct scatterlist), GFP_ATOMIC);
+	for (i = 0; i < periods; i++) {
+		sg_dma_address(&sg[i]) = dma_addr;
+		sg_dma_len(&sg[i]) = period_len;
+		dma_addr += period_len;
 	}
 
-	cdesc = stedma40_cyclic_prep_sg(chan, sg, sg_len, direction,
-					DMA_PREP_INTERRUPT);
-	kfree(sg);
-
-	if (IS_ERR(cdesc))
-		return ERR_PTR(PTR_ERR(cdesc));
+	sg[periods].offset = 0;
+	sg[periods].length = 0;
+	sg[periods].page_link =
+		((unsigned long)sg | 0x01) & ~0x02;
 
-	txd = &cdesc->d40d->txd;
+	txd = d40_prep_sg(chan, sg, sg, periods, direction,
+			  DMA_PREP_INTERRUPT);
 
-	txd->flags = DMA_PREP_INTERRUPT;
-	dma_async_tx_descriptor_init(txd, chan);
-	txd->tx_submit = d40_tx_submit;
+	kfree(sg);
 
 	return txd;
 }
@@ -3142,6 +2976,13 @@ static int __init d40_phy_res_init(struct d40_base *base)
 		num_phy_chans_avail--;
 	}
 
+	/* Mark soft_lli channels */
+	for (i = 0; i < base->plat_data->num_of_soft_lli_chans; i++) {
+		int chan = base->plat_data->soft_lli_chans[i];
+
+		base->phy_res[chan].use_soft_lli = true;
+	}
+
 	dev_info(base->dev, "%d of %d physical DMA channels available\n",
 		 num_phy_chans_avail, base->num_phy_chans);
 
diff --git a/drivers/gpu/ion/ion.c b/drivers/gpu/ion/ion.c
index 9cb5b25bb11..37b23af0550 100644
--- a/drivers/gpu/ion/ion.c
+++ b/drivers/gpu/ion/ion.c
@@ -131,7 +131,7 @@ static void ion_buffer_add(struct ion_device *dev,
 }
 
 /* this function should only be called while dev->lock is held */
-struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
+static struct ion_buffer *ion_buffer_create(struct ion_heap *heap,
 				     struct ion_device *dev,
 				     unsigned long len,
 				     unsigned long align,
@@ -181,7 +181,7 @@ 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,
+static struct ion_handle *ion_handle_create(struct ion_client *client,
 				     struct ion_buffer *buffer)
 {
 	struct ion_handle *handle;
@@ -190,6 +190,7 @@ struct ion_handle *ion_handle_create(struct ion_client *client,
 	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;
@@ -205,7 +206,8 @@ static void ion_handle_destroy(struct kref *kref)
 	 */
 	ion_buffer_put(handle->buffer);
 	mutex_lock(&handle->client->lock);
-	rb_erase(&handle->node, &handle->client->handles);
+	if (!RB_EMPTY_NODE(&handle->node))
+		rb_erase(&handle->node, &handle->client->handles);
 	mutex_unlock(&handle->client->lock);
 	kfree(handle);
 }
@@ -239,7 +241,7 @@ static struct ion_handle *ion_handle_lookup(struct ion_client *client,
 	return NULL;
 }
 
-bool ion_handle_validate(struct ion_client *client, struct ion_handle *handle)
+static bool ion_handle_validate(struct ion_client *client, struct ion_handle *handle)
 {
 	struct rb_node *n = client->handles.rb_node;
 
@@ -351,7 +353,7 @@ void ion_free(struct ion_client *client, struct ion_handle *handle)
 static void ion_client_get(struct ion_client *client);
 static int ion_client_put(struct ion_client *client);
 
-bool _ion_map(int *buffer_cnt, int *handle_cnt)
+static bool _ion_map(int *buffer_cnt, int *handle_cnt)
 {
 	bool map;
 
@@ -367,7 +369,7 @@ bool _ion_map(int *buffer_cnt, int *handle_cnt)
 	return map;
 }
 
-bool _ion_unmap(int *buffer_cnt, int *handle_cnt)
+static bool _ion_unmap(int *buffer_cnt, int *handle_cnt)
 {
 	BUG_ON(*handle_cnt == 0);
 	(*handle_cnt)--;
@@ -522,7 +524,7 @@ struct ion_buffer *ion_share(struct ion_client *client,
 		return ERR_PTR(-EINVAL);
 	}
 
-	/* don't not take an extra refernce here, the burden is on the caller
+	/* 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
@@ -897,7 +899,7 @@ err1:
 	/* drop the reference to the handle */
 	ion_handle_put(handle);
 err:
-	/* drop the refernce to the client */
+	/* drop the reference to the client */
 	ion_client_put(client);
 	return ret;
 }
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index bae48745bb4..9a243ca96e6 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-magicmouse.c b/drivers/hid/hid-magicmouse.c
index 0ec91c18a42..56d0539f2a3 100644
--- a/drivers/hid/hid-magicmouse.c
+++ b/drivers/hid/hid-magicmouse.c
@@ -501,9 +501,17 @@ static int magicmouse_probe(struct hid_device *hdev,
 	}
 	report->size = 6;
 
+	/*
+	 * Some devices repond with 'invalid report id' when feature
+	 * report switching it into multitouch mode is sent to it.
+	 *
+	 * This results in -EIO from the _raw low-level transport callback,
+	 * but there seems to be no other way of switching the mode.
+	 * Thus the super-ugly hacky success check below.
+	 */
 	ret = hdev->hid_output_raw_report(hdev, feature, sizeof(feature),
 			HID_FEATURE_REPORT);
-	if (ret != sizeof(feature)) {
+	if (ret != -EIO && ret != sizeof(feature)) {
 		hid_err(hdev, "unable to request touch data (%d)\n", ret);
 		goto err_stop_hw;
 	}
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 62cac4dc3b6..5de25ff1cc3 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:
diff --git a/drivers/hwmon/lsm303dlh_a.c b/drivers/hwmon/lsm303dlh_a.c
index 4f58e2a75aa..d14468618ee 100644
--- a/drivers/hwmon/lsm303dlh_a.c
+++ b/drivers/hwmon/lsm303dlh_a.c
@@ -837,10 +837,15 @@ static ssize_t lsm303dlh_a_store_mode(struct device *dev,
 
 	data = lsm303dlh_a_read(ddata, CTRL_REG1, "CTRL_REG1");
 
+	/*
+	 * If chip doesn't get reset during suspend/resume,
+	 * x,y and z axis bits are getting cleared,so set
+	 * these bits to get x,y,z axis data.
+	 */
+	data |= LSM303DLH_A_CR1_AXIS_ENABLE;
 	data &= ~LSM303DLH_A_CR1_PM_MASK;
 
 	ddata->mode = val;
-
 	data |= ((val << LSM303DLH_A_CR1_PM_BIT) & LSM303DLH_A_CR1_PM_MASK);
 
 	error = lsm303dlh_a_write(ddata, CTRL_REG1, data, "CTRL_REG1");
diff --git a/drivers/hwmon/lsm303dlhc_a.c b/drivers/hwmon/lsm303dlhc_a.c
index 17c74595ff2..a8b1cd95fa9 100644
--- a/drivers/hwmon/lsm303dlhc_a.c
+++ b/drivers/hwmon/lsm303dlhc_a.c
@@ -400,6 +400,13 @@ static ssize_t lsm303dlhc_a_store_mode(struct device *dev,
 
 	data = lsm303dlhc_a_read(ddata, CTRL_REG1, "CTRL_REG1");
 
+	/*
+	 * If chip doesn't get reset during suspend/resume,
+	 * x,y and z axis bits are getting cleared,so set
+	 * these bits to get x,y,z data.
+	 */
+	data |= LSM303DLHC_A_CR1_AXIS_ENABLE;
+
 	data &= ~LSM303DLHC_A_CR1_MODE_MASK;
 
 	ddata->mode = val;
@@ -518,6 +525,11 @@ static int __devinit lsm303dlhc_a_probe(struct i2c_client *client,
 	}
 
 	if (adata->regulator) {
+		/*
+		 * 130 microamps typical with magnetic sensor setting ODR = 7.5
+		 * Hz, Accelerometer sensor ODR = 50 Hz.  Double for safety.
+		 */
+		regulator_set_optimum_mode(adata->regulator, 130 * 2);
 		regulator_enable(adata->regulator);
 		adata->device_status = DEVICE_ON;
 	}
diff --git a/drivers/input/misc/abx500-accdet.c b/drivers/input/misc/abx500-accdet.c
index b0879b6c96a..6ce49c7f682 100644
--- a/drivers/input/misc/abx500-accdet.c
+++ b/drivers/input/misc/abx500-accdet.c
@@ -53,9 +53,9 @@
 #define ACCESSORY_CVIDEO_DET_VOL_MAX	105
 #define ACCESSORY_CARKIT_DET_VOL_MIN	1100
 #define ACCESSORY_CARKIT_DET_VOL_MAX	1300
-#define ACCESSORY_HEADSET_DET_VOL_MIN	0
-#define ACCESSORY_HEADSET_DET_VOL_MAX	200
-#define ACCESSORY_OPENCABLE_DET_VOL_MIN	1730
+#define ACCESSORY_HEADSET_DET_VOL_MIN	1301
+#define ACCESSORY_HEADSET_DET_VOL_MAX	2000
+#define ACCESSORY_OPENCABLE_DET_VOL_MIN	2001
 #define ACCESSORY_OPENCABLE_DET_VOL_MAX	2150
 
 
@@ -522,8 +522,6 @@ static int detect_hw(struct abx500_ad *dd,
 		status = dd->detect_plugged_in(dd);
 		break;
 	case JACK_TYPE_CARKIT:
-		dd->config_hw_test_basic_carkit(dd, 1);
-		/* flow through.. */
 	case JACK_TYPE_HEADPHONE:
 	case JACK_TYPE_CVIDEO:
 	case JACK_TYPE_HEADSET:
@@ -550,6 +548,8 @@ static enum accessory_jack_type detect(struct abx500_ad *dd,
 	int i;
 
 	accessory_regulator_enable(dd, REGULATOR_VAUDIO | REGULATOR_AVSWITCH);
+	/* enable the VAMIC1 regulator */
+	dd->config_hw_test_basic_carkit(dd, 0);
 
 	for (i = 0; i < ARRAY_SIZE(detect_ops); ++i) {
 		if (detect_hw(dd, &detect_ops[i])) {
@@ -559,7 +559,6 @@ static enum accessory_jack_type detect(struct abx500_ad *dd,
 		}
 	}
 
-	dd->config_hw_test_basic_carkit(dd, 0);
 	dd->config_hw_test_plug_connected(dd, 0);
 
 	if (jack_supports_buttons(type))
diff --git a/drivers/input/misc/lps001wp_prs.c b/drivers/input/misc/lps001wp_prs.c
index 9ec96ba3863..cb60762ac61 100644
--- a/drivers/input/misc/lps001wp_prs.c
+++ b/drivers/input/misc/lps001wp_prs.c
@@ -46,6 +46,7 @@
 #include <linux/input.h>
 #include <linux/workqueue.h>
 #include <linux/device.h>
+#include <linux/regulator/consumer.h>
 
 #include <linux/input/lps001wp.h>
 
@@ -164,6 +165,8 @@ struct lps001wp_prs_data {
 
 	u8 resume_state[RESUME_ENTRIES];
 
+	struct regulator *regulator;
+
 #ifdef DEBUG
 	u8 reg_addr;
 #endif
@@ -380,10 +383,13 @@ static void lps001wp_prs_device_power_off(struct lps001wp_prs_data *prs)
 	if (err < 0)
 		dev_err(&prs->client->dev, "soft power off failed: %d\n", err);
 
-	if (prs->pdata->power_off) {
-		prs->pdata->power_off();
-		prs->hw_initialized = 0;
+	/* disable regulator */
+	if (prs->regulator) {
+		err = regulator_disable(prs->regulator);
+		if (err < 0)
+			dev_err(&prs->client->dev, "failed to disable regulator\n");
 	}
+
 	if (prs->hw_initialized) {
 		prs->hw_initialized = 0;
 	}
@@ -394,15 +400,23 @@ static int lps001wp_prs_device_power_on(struct lps001wp_prs_data *prs)
 {
 	int err = -1;
 
-	if (prs->pdata->power_on) {
-		err = prs->pdata->power_on();
-		if (err < 0) {
-			dev_err(&prs->client->dev,
-					"power_on failed: %d\n", err);
-			return err;
+	/* get the regulator the first time */
+	if (!prs->regulator) {
+		prs->regulator = regulator_get(&prs->client->dev, "vdd");
+		if (IS_ERR(prs->regulator)) {
+			dev_err(&prs->client->dev, "failed to get regulator\n");
+			prs->regulator = NULL;
+			return PTR_ERR(prs->regulator);
 		}
 	}
 
+	/* enable it also */
+	err = regulator_enable(prs->regulator);
+	if (err < 0) {
+		dev_err(&prs->client->dev, "failed to enable regulator\n");
+		return err;
+	}
+
 	if (!prs->hw_initialized) {
 		err = lps001wp_prs_hw_init(prs);
 		if (prs->hw_working == 1 && err < 0) {
@@ -1210,6 +1224,10 @@ static int __devexit lps001wp_prs_remove(struct i2c_client *client)
 
 	if (prs->pdata->exit)
 		prs->pdata->exit();
+
+	if (prs->regulator)
+		regulator_put(prs->regulator);
+
 	kfree(prs->pdata);
 	kfree(prs);
 
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index eed56a4cfab..9b614f55c77 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -94,6 +94,9 @@
 
 #define AB8500_TURN_ON_STATUS		0x00
 
+static bool no_bm; /* No battery management */
+module_param(no_bm, bool, S_IRUGO);
+
 /*
  * Map interrupt numbers to the LATCH and MASK register offsets, Interrupt
  * numbers are indexed into this array with (num / 8).
@@ -720,26 +723,6 @@ static struct mfd_cell __devinitdata ab8500_devs[] = {
 		.resources = ab8500_rtc_resources,
 	},
 	{
-		.name = "ab8500-charger",
-		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
-		.resources = ab8500_charger_resources,
-	},
-	{
-		.name = "ab8500-btemp",
-		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
-		.resources = ab8500_btemp_resources,
-	},
-	{
-		.name = "ab8500-fg",
-		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
-		.resources = ab8500_fg_resources,
-	},
-	{
-		.name = "ab8500-chargalg",
-		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
-		.resources = ab8500_chargalg_resources,
-	},
-	{
 		.name = "ab8500-acc-det",
 		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
 		.resources = ab8500_av_acc_detect_resources,
@@ -780,6 +763,29 @@ static struct mfd_cell __devinitdata ab8500_devs[] = {
 	},
 };
 
+static struct mfd_cell __devinitdata ab8500_bm_devs[] = {
+	{
+		.name = "ab8500-charger",
+		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
+		.resources = ab8500_charger_resources,
+	},
+	{
+		.name = "ab8500-btemp",
+		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
+		.resources = ab8500_btemp_resources,
+	},
+	{
+		.name = "ab8500-fg",
+		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
+		.resources = ab8500_fg_resources,
+	},
+	{
+		.name = "ab8500-chargalg",
+		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
+		.resources = ab8500_chargalg_resources,
+	},
+};
+
 static ssize_t show_chip_id(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
@@ -945,9 +951,19 @@ int __devinit ab8500_init(struct ab8500 *ab8500)
 	ret = mfd_add_devices(ab8500->dev, 0, ab8500_devs,
 			      ARRAY_SIZE(ab8500_devs), NULL,
 			      ab8500->irq_base);
+
 	if (ret)
 		goto out_freeirq;
 
+	if (!no_bm) {
+		/* Add battery management devices */
+		ret = mfd_add_devices(ab8500->dev, 0, ab8500_bm_devs,
+				      ARRAY_SIZE(ab8500_bm_devs), NULL,
+				      ab8500->irq_base);
+		if (ret)
+			dev_err(ab8500->dev, "error adding bm devices\n");
+	}
+
 	ret = sysfs_create_group(&ab8500->dev->kobj, &ab8500_attr_group);
 	if (ret)
 		dev_err(ab8500->dev, "error creating sysfs entries\n");
diff --git a/drivers/mfd/db5500-prcmu.c b/drivers/mfd/db5500-prcmu.c
index 55560abe001..2633b728ada 100644
--- a/drivers/mfd/db5500-prcmu.c
+++ b/drivers/mfd/db5500-prcmu.c
@@ -1995,6 +1995,16 @@ static struct regulator_consumer_supply db5500_esram12_consumers[] = {
 	.num_consumer_supplies  = ARRAY_SIZE(db5500_##lower##_consumers),\
 }
 
+#define DB5500_REGULATOR_SWITCH_VAPE(lower, upper)			\
+[DB5500_REGULATOR_SWITCH_##upper] = {					\
+	.supply_regulator = "db5500-vape",				\
+	.constraints = {						\
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,		\
+	},								\
+	.consumer_supplies      = db5500_##lower##_consumers,		\
+	.num_consumer_supplies  = ARRAY_SIZE(db5500_##lower##_consumers),\
+}									\
+
 static struct regulator_init_data db5500_regulators[DB5500_NUM_REGULATORS] = {
 	[DB5500_REGULATOR_VAPE] = {
 		.constraints = {
@@ -2003,10 +2013,14 @@ static struct regulator_init_data db5500_regulators[DB5500_NUM_REGULATORS] = {
 		.consumer_supplies	= db5500_vape_consumers,
 		.num_consumer_supplies	= ARRAY_SIZE(db5500_vape_consumers),
 	},
-	DB5500_REGULATOR_SWITCH(sga, SGA),
-	DB5500_REGULATOR_SWITCH(hva, HVA),
-	DB5500_REGULATOR_SWITCH(sia, SIA),
-	DB5500_REGULATOR_SWITCH(disp, DISP),
+	DB5500_REGULATOR_SWITCH_VAPE(sga, SGA),
+	DB5500_REGULATOR_SWITCH_VAPE(hva, HVA),
+	DB5500_REGULATOR_SWITCH_VAPE(sia, SIA),
+	DB5500_REGULATOR_SWITCH_VAPE(disp, DISP),
+	/*
+	 * ESRAM12 is put in retention by the firmware when VAPE is
+	 * turned off so there's no need to hold VAPE.
+	 */
 	DB5500_REGULATOR_SWITCH(esram12, ESRAM12),
 };
 
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index ab9ee25dabd..ccec631c38f 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -412,8 +412,8 @@ static struct {
 static atomic_t ac_wake_req_state = ATOMIC_INIT(0);
 
 /* Spinlocks */
+static DEFINE_SPINLOCK(prcmu_lock);
 static DEFINE_SPINLOCK(clkout_lock);
-static DEFINE_SPINLOCK(gpiocr_lock);
 
 /* Global var to runtime determine TCDM base for v2 or v1 */
 static __iomem void *tcdm_base;
@@ -642,94 +642,30 @@ int db8500_prcmu_set_display_clocks(void)
 	return 0;
 }
 
-/**
- * prcmu_enable_spi2 - Enables pin muxing for SPI2 on OtherAlternateC1.
- */
-void prcmu_enable_spi2(void)
-{
-	u32 reg;
-	unsigned long flags;
-
-	spin_lock_irqsave(&gpiocr_lock, flags);
-	reg = readl(PRCM_GPIOCR);
-	writel(reg | PRCM_GPIOCR_SPI2_SELECT, PRCM_GPIOCR);
-	spin_unlock_irqrestore(&gpiocr_lock, flags);
-}
-
-/**
- * prcmu_disable_spi2 - Disables pin muxing for SPI2 on OtherAlternateC1.
- */
-void prcmu_disable_spi2(void)
-{
-	u32 reg;
-	unsigned long flags;
-
-	spin_lock_irqsave(&gpiocr_lock, flags);
-	reg = readl(PRCM_GPIOCR);
-	writel(reg & ~PRCM_GPIOCR_SPI2_SELECT, PRCM_GPIOCR);
-	spin_unlock_irqrestore(&gpiocr_lock, flags);
-}
-
-/**
- * prcmu_enable_stm_mod_uart - Enables pin muxing for STMMOD
- * and UARTMOD on OtherAlternateC3.
- */
-void prcmu_enable_stm_mod_uart(void)
+u32 db8500_prcmu_read(unsigned int reg)
 {
-	u32 reg;
-	unsigned long flags;
-
-	spin_lock_irqsave(&gpiocr_lock, flags);
-	reg = readl(PRCM_GPIOCR);
-	reg |= (PRCM_GPIOCR_DBG_STM_MOD_SELECT
-			| PRCM_GPIOCR_DBG_UARTMOD_SELECT);
-	writel(reg, PRCM_GPIOCR);
-	spin_unlock_irqrestore(&gpiocr_lock, flags);
+	return readl(_PRCMU_BASE + reg);
 }
 
-/**
- * prcmu_enable_stm_mod_uart - Disables pin muxing for STMMOD
- * and UARTMOD on OtherAlternateC3.
- */
-void prcmu_disable_stm_mod_uart(void)
+void db8500_prcmu_write(unsigned int reg, u32 value)
 {
-	u32 reg;
 	unsigned long flags;
 
-	spin_lock_irqsave(&gpiocr_lock, flags);
-	reg = readl(PRCM_GPIOCR);
-	reg &= ~(PRCM_GPIOCR_DBG_STM_MOD_SELECT
-			| PRCM_GPIOCR_DBG_UARTMOD_SELECT);
-	writel(reg, PRCM_GPIOCR);
-	spin_unlock_irqrestore(&gpiocr_lock, flags);
+	spin_lock_irqsave(&prcmu_lock, flags);
+	writel(value, (_PRCMU_BASE + reg));
+	spin_unlock_irqrestore(&prcmu_lock, flags);
 }
 
-/**
- * prcmu_enable_stm_ape - Enables pin muxing for STM APE on OtherAlternateC1.
- */
-void prcmu_enable_stm_ape(void)
+void db8500_prcmu_write_masked(unsigned int reg, u32 mask, u32 value)
 {
-	u32 reg;
-	unsigned long flags;
-
-	spin_lock_irqsave(&gpiocr_lock, flags);
-	reg = readl(PRCM_GPIOCR);
-	writel(reg | PRCM_GPIOCR_DBG_STM_APE_SELECT, PRCM_GPIOCR);
-	spin_unlock_irqrestore(&gpiocr_lock, flags);
-}
-
-/**
- * prcmu_disable_stm_ape - Disables pin muxing for STM APE on OtherAlternateC1.
- */
-void prcmu_disable_stm_ape(void)
-{
-	u32 reg;
+	u32 val;
 	unsigned long flags;
 
-	spin_lock_irqsave(&gpiocr_lock, flags);
-	reg = readl(PRCM_GPIOCR);
-	writel(reg & ~PRCM_GPIOCR_DBG_STM_APE_SELECT, PRCM_GPIOCR);
-	spin_unlock_irqrestore(&gpiocr_lock, flags);
+	spin_lock_irqsave(&prcmu_lock, flags);
+	val = readl(_PRCMU_BASE + reg);
+	val = ((val & ~mask) | (value & mask));
+	writel(val, (_PRCMU_BASE + reg));
+	spin_unlock_irqrestore(&prcmu_lock, flags);
 }
 
 bool prcmu_has_arm_maxopp(void)
@@ -1594,7 +1530,8 @@ static int request_plldsi(bool enable)
 	int r = 0;
 	u32 val;
 
-	writel(PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMP, (enable ?
+	writel((PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMP |
+		PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMPI), (enable ?
 		PRCM_MMIP_LS_CLAMP_CLR : PRCM_MMIP_LS_CLAMP_SET));
 
 	val = readl(PRCM_PLLDSI_ENABLE);
@@ -1616,7 +1553,8 @@ static int request_plldsi(bool enable)
 			writel(PRCM_APE_RESETN_DSIPLL_RESETN,
 				PRCM_APE_RESETN_SET);
 		} else {
-			writel(PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMP,
+			writel((PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMP |
+				PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMPI),
 				PRCM_MMIP_LS_CLAMP_SET);
 			val &= ~PRCM_PLLDSI_ENABLE_PRCM_PLLDSI_ENABLE;
 			writel(val, PRCM_PLLDSI_ENABLE);
diff --git a/drivers/mfd/dbx500-prcmu-regs.h b/drivers/mfd/dbx500-prcmu-regs.h
index 9d0bc7200c0..4c5c2478c05 100644
--- a/drivers/mfd/dbx500-prcmu-regs.h
+++ b/drivers/mfd/dbx500-prcmu-regs.h
@@ -131,7 +131,8 @@
 #define PRCM_MMIP_LS_CLAMP_SET     (_PRCMU_BASE + 0x420)
 #define PRCM_MMIP_LS_CLAMP_CLR     (_PRCMU_BASE + 0x424)
 
-#define PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMP BIT(11)
+#define PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMP		BIT(11)
+#define PRCM_MMIP_LS_CLAMP_DSIPLL_CLAMPI	BIT(22)
 
 /* PRCMU clock/PLL/reset registers */
 #define PRCM_PLLSOC0_FREQ	   (_PRCMU_BASE + 0x080)
diff --git a/drivers/misc/mbox_channels-db5500.c b/drivers/misc/mbox_channels-db5500.c
index 43a4d57caaf..b3baae12d4b 100644
--- a/drivers/misc/mbox_channels-db5500.c
+++ b/drivers/misc/mbox_channels-db5500.c
@@ -379,7 +379,7 @@ rcv_msg:
 		dev_err(&channels.pdev->dev,
 				"%s no callback provided\n", __func__);
 	}
-	if (atomic_dec_and_test(&rx_chan->rcv_counter) > 0)
+	if (!atomic_dec_and_test(&rx_chan->rcv_counter))
 		goto rcv_msg;
 
 }
diff --git a/drivers/misc/modem_audio/mad.c b/drivers/misc/modem_audio/mad.c
index 62bef3c75fd..88bbcaa3c9c 100644
--- a/drivers/misc/modem_audio/mad.c
+++ b/drivers/misc/modem_audio/mad.c
@@ -459,19 +459,15 @@ static int mad_close(struct inode *ino, struct file *filp)
 {
 	dev_dbg(mad_dev.this_device, "%s", __func__);
 
-	if (mad->dsp_shm_write_ptr != NULL) {
-		iounmap(mad->dsp_shm_write_ptr);
-		mad->dsp_shm_write_ptr = NULL;
-	}
-
-	if (mad->dsp_shm_read_ptr != NULL) {
-		iounmap(mad->dsp_shm_read_ptr);
-		mad->dsp_shm_read_ptr = NULL;
+	if (mbox_channel_deregister(CHANNEL_NUM_RX)) {
+		dev_err(mad_dev.this_device, "%s:deregister err", __func__);
+		return -EFAULT;
 	}
-
 	kfree(mad->rx_buff);
 	kfree(mad->tx_buff);
 	mad->data_written = 0;
+	mad->rx_buffer_num = 0;
+	mad->rx_buffer_read = 0;
 	mad->open_check = false;
 
 	return 0;
@@ -495,7 +491,16 @@ static void  __exit mad_exit(void)
 {
 	dev_dbg(mad_dev.this_device, "%s", __func__);
 
-	kfree(mad);
+	 if (mad->dsp_shm_write_ptr != NULL) {
+		 iounmap(mad->dsp_shm_write_ptr);
+		 mad->dsp_shm_write_ptr = NULL;
+	 }
+
+	 if (mad->dsp_shm_read_ptr != NULL) {
+		 iounmap(mad->dsp_shm_read_ptr);
+		 mad->dsp_shm_read_ptr = NULL;
+	 }
 
-	misc_deregister(&mad_dev);
+	 kfree(mad);
+	 misc_deregister(&mad_dev);
 }
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index dc783c0ac22..cf738772ec1 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -1700,6 +1700,13 @@ static const struct mmc_fixup blk_fixups[] =
 		  MMC_QUIRK_BLK_NO_CMD23),
 	MMC_FIXUP("MMC32G", 0x11, CID_OEMID_ANY, add_quirk_mmc,
 		  MMC_QUIRK_BLK_NO_CMD23),
+
+	/*
+	 * Some Micron MMC cards needs longer data read timeout than
+	 * indicated in CSD.
+	 */
+	MMC_FIXUP("", 0x13, 0x200, add_quirk_mmc, MMC_QUIRK_LONG_READ_TIME),
+
 	END_FIXUP
 };
 
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 11907e15e5f..214ace6b905 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -471,6 +471,18 @@ void mmc_set_data_timeout(struct mmc_data *data, const struct mmc_card *card)
 			data->timeout_clks = 0;
 		}
 	}
+
+	/*
+	 * Some cards require longer data read timeout than indicated in CSD.
+	 * Address this by setting the read timeout to a "reasonably high"
+	 * value. For the cards tested, 300ms has proven enough. If necessary,
+	 * this value can be increased if other problematic cards require this.
+	 */
+	if (mmc_card_long_read_time(card) && data->flags & MMC_DATA_READ) {
+		data->timeout_ns = 300000000;
+		data->timeout_clks = 0;
+	}
+
 	/*
 	 * Some cards need very high timeouts if driven in SPI mode.
 	 * The worst observed timeout was 900ms after writing a
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index f235aa45a3e..704163b3bc5 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -751,7 +751,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 */
@@ -1031,8 +1032,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;
@@ -1579,10 +1581,25 @@ static int mmci_runtime_suspend(struct device *dev)
 	struct amba_device *adev = to_amba_device(dev);
 	struct mmc_host *mmc = amba_get_drvdata(adev);
 	unsigned long flags;
+	int ret;
+	struct mmc_ios ios;
 
 	if (mmc) {
 		struct mmci_host *host = mmc_priv(mmc);
 
+		/*
+		 * Let the ios_handler act on a POWER_OFF to potentially do some
+		 * power save actions.
+		 */
+		if (host->plat->ios_handler) {
+			memcpy(&ios, &mmc->ios, sizeof(struct mmc_ios));
+			ios.power_mode = MMC_POWER_OFF;
+			ret = host->plat->ios_handler(mmc_dev(mmc),
+						      &ios);
+			if (ret)
+				return ret;
+		}
+
 		spin_lock_irqsave(&host->lock, flags);
 
 		/* Save registers for POWER, CLOCK and IRQMASK0 */
@@ -1628,6 +1645,14 @@ static int mmci_runtime_resume(struct device *dev)
 		writel(host->irqmask0_reg, host->base + MMCIMASK0);
 
 		spin_unlock_irqrestore(&host->lock, flags);
+
+		/*
+		 * Restore settings done by the ios_handler. This shall be done
+		 * quickly to keep request latency low.
+		 */
+		if (host->plat->ios_handler)
+			host->plat->ios_handler(mmc_dev(mmc),
+						&mmc->ios);
 	}
 
 	return 0;
diff --git a/drivers/net/smsc911x.c b/drivers/net/smsc911x.c
index d9a16744e2f..df41f57c85d 100644
--- a/drivers/net/smsc911x.c
+++ b/drivers/net/smsc911x.c
@@ -52,6 +52,8 @@
 #include <linux/phy.h>
 #include <linux/smsc911x.h>
 #include <linux/device.h>
+#include <linux/regulator/consumer.h>
+
 #include "smsc911x.h"
 
 #define SMSC_CHIPNAME		"smsc911x"
@@ -132,6 +134,10 @@ struct smsc911x_data {
 
 	/* register access functions */
 	const struct smsc911x_ops *ops;
+
+	/* regulators */
+	struct regulator *regulator_vddvario;
+	struct regulator *regulator_vdd33a;
 };
 
 /* Easy access to information */
@@ -364,6 +370,81 @@ out:
 	spin_unlock_irqrestore(&pdata->dev_lock, flags);
 }
 
+/* Enable resources(clocks and regulators) */
+static int smsc911x_enable_resources(struct platform_device *pdev, bool enable)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct smsc911x_data *pdata = netdev_priv(ndev);
+	int err = 0;
+
+	/* enable/diable regulator for vddvario */
+	if (pdata->regulator_vddvario) {
+		if (enable) {
+			err = regulator_enable(pdata->regulator_vddvario);
+			if (err < 0) {
+				netdev_err(ndev, "%s: regulator_enable failed '%s'\n",
+						__func__, "vddvario");
+			}
+		} else
+			err = regulator_disable(pdata->regulator_vdd33a);
+	}
+
+	/* enable/diableregulator for vdd33a */
+	if (pdata->regulator_vdd33a) {
+		if (enable) {
+			err = regulator_enable(pdata->regulator_vdd33a);
+			if (err < 0) {
+				netdev_err(ndev, "%s: regulator_enable failed '%s'\n",
+						__func__, "vdd33a");
+			}
+		} else
+			err = regulator_disable(pdata->regulator_vdd33a);
+	}
+	return err;
+}
+
+
+/* Request resources(clocks and regulators) */
+static int smsc911x_request_resources(struct platform_device *pdev,
+		bool request)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct smsc911x_data *pdata = netdev_priv(ndev);
+	int err = 0;
+
+	/* Request regulator for vddvario */
+	if (request && !pdata->regulator_vddvario) {
+		pdata->regulator_vddvario = regulator_get(&pdev->dev,
+				"vddvario");
+		if (IS_ERR(pdata->regulator_vddvario)) {
+			netdev_warn(ndev,
+					"%s: Failed to get regulator '%s'\n",
+					__func__, "vddvario");
+			pdata->regulator_vddvario = NULL;
+		}
+	} else if (!request && pdata->regulator_vddvario) {
+		regulator_put(pdata->regulator_vddvario);
+		pdata->regulator_vddvario = NULL;
+	}
+
+	/* Request regulator for vdd33a */
+	if (request && !pdata->regulator_vddvario) {
+		pdata->regulator_vdd33a = regulator_get(&pdev->dev,
+				"vdd33a");
+		if (IS_ERR(pdata->regulator_vdd33a)) {
+			netdev_warn(ndev,
+					"%s: Failed to get regulator '%s'\n",
+					__func__, "vdd33a");
+			pdata->regulator_vdd33a = NULL;
+		}
+	} else if (!request && pdata->regulator_vdd33a) {
+		regulator_put(pdata->regulator_vdd33a);
+		pdata->regulator_vdd33a = NULL;
+	}
+
+	return err;
+}
+
 /* waits for MAC not busy, with timeout.  Only called by smsc911x_mac_read
  * and smsc911x_mac_write, so assumes mac_lock is held */
 static int smsc911x_mac_complete(struct smsc911x_data *pdata)
@@ -2053,6 +2134,7 @@ static int __devexit smsc911x_drv_remove(struct platform_device *pdev)
 	struct net_device *dev;
 	struct smsc911x_data *pdata;
 	struct resource *res;
+	int retval;
 
 	dev = platform_get_drvdata(pdev);
 	BUG_ON(!dev);
@@ -2080,6 +2162,12 @@ static int __devexit smsc911x_drv_remove(struct platform_device *pdev)
 
 	iounmap(pdata->ioaddr);
 
+	if (smsc911x_enable_resources(pdev, false))
+		pr_warn("Could not disable resource\n");
+
+	retval = smsc911x_request_resources(pdev, false);
+	/* ignore not all have regulators */
+
 	free_netdev(dev);
 
 	return 0;
@@ -2110,6 +2198,7 @@ static int __devinit smsc911x_drv_probe(struct platform_device *pdev)
 	unsigned int intcfg = 0;
 	int res_size, irq_flags;
 	int retval;
+	int to = 100;
 
 	pr_info("Driver version %s\n", SMSC_DRV_VERSION);
 
@@ -2165,6 +2254,17 @@ static int __devinit smsc911x_drv_probe(struct platform_device *pdev)
 	pdata->dev = dev;
 	pdata->msg_enable = ((1 << debug) - 1);
 
+	platform_set_drvdata(pdev, dev);
+
+	retval = smsc911x_request_resources(pdev, true);
+	/* ignore not all have regulators */
+
+	retval = smsc911x_enable_resources(pdev, true);
+	if (retval) {
+		pr_warn("Could not enable resource\n");
+		goto out_0;
+	}
+
 	if (pdata->ioaddr == NULL) {
 		SMSC_WARN(pdata, probe, "Error smsc911x base address invalid");
 		retval = -ENOMEM;
@@ -2177,6 +2277,18 @@ static int __devinit smsc911x_drv_probe(struct platform_device *pdev)
 	if (config->shift)
 		pdata->ops = &shifted_smsc911x_ops;
 
+	/* poll the READY bit in PMT_CTRL. Any other access to the device is
+	 * forbidden while this bit isn't set. Try for 100ms
+	 */
+	while (!(smsc911x_reg_read(pdata, PMT_CTRL) & PMT_CTRL_READY_) && --to)
+		udelay(1000);
+
+	if (to == 0) {
+		pr_err("Device not READY in 100ms aborting\n");
+		goto out_0;
+	}
+
+
 	retval = smsc911x_init(dev);
 	if (retval < 0)
 		goto out_unmap_io_3;
@@ -2269,6 +2381,7 @@ out_0:
 	return retval;
 }
 
+
 #ifdef CONFIG_PM
 /* This implementation assumes the devices remains powered on its VDDVARIO
  * pins during suspend. */
diff --git a/drivers/net/wireless/bcmdhd/Makefile b/drivers/net/wireless/bcmdhd/Makefile
index 56500b7220c..e82c9856f00 100644
--- a/drivers/net/wireless/bcmdhd/Makefile
+++ b/drivers/net/wireless/bcmdhd/Makefile
@@ -1,5 +1,5 @@
 # bcmdhd
-DHDCFLAGS = -Wall -Wstrict-prototypes -Werror -Dlinux -DBCMDRIVER             \
+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               \
diff --git a/drivers/net/wireless/bcmdhd/bcmevent.c b/drivers/net/wireless/bcmdhd/bcmevent.c
index 3cee7c214e3..24581ddd353 100644
--- a/drivers/net/wireless/bcmdhd/bcmevent.c
+++ b/drivers/net/wireless/bcmdhd/bcmevent.c
@@ -29,7 +29,7 @@
 #include <proto/bcmeth.h>
 #include <proto/bcmevent.h>
 
-#if WLC_E_LAST != 84
+#if WLC_E_LAST != 85
 #error "You need to add an entry to bcmevent_names[] for the new event"
 #endif
 
@@ -115,7 +115,10 @@ const bcmevent_name_t bcmevent_names[] = {
 	{ 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" }
+	{ WLC_E_PFN_SCAN_ALLGONE, "PFN_SCAN_ALLGONE" },
+#ifdef SOFTAP
+	{ WLC_E_GTK_PLUMBED, "GTK_PLUMBED" }
+#endif
 };
 
 
diff --git a/drivers/net/wireless/bcmdhd/bcmsdh_linux.c b/drivers/net/wireless/bcmdhd/bcmsdh_linux.c
index 6fa47378cfe..a4dc6ff4042 100644
--- a/drivers/net/wireless/bcmdhd/bcmsdh_linux.c
+++ b/drivers/net/wireless/bcmdhd/bcmsdh_linux.c
@@ -48,15 +48,6 @@ extern void dhdsdio_isr(void * args);
 #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
diff --git a/drivers/net/wireless/bcmdhd/dhd.h b/drivers/net/wireless/bcmdhd/dhd.h
index ea8f67ce8b4..281e047af85 100644
--- a/drivers/net/wireless/bcmdhd/dhd.h
+++ b/drivers/net/wireless/bcmdhd/dhd.h
@@ -71,6 +71,12 @@ enum dhd_bus_state {
 	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
+
 enum dhd_bus_wake_state {
 	WAKE_LOCK_OFF,
 	WAKE_LOCK_PRIV,
@@ -173,6 +179,7 @@ typedef struct dhd_pub {
 
 	wl_country_t dhd_cspec;		/* Current Locale info */
 	char eventmask[WL_EVENTING_MASK_LEN];
+	int	op_mode;				/* STA, HostAPD, WFD, SoftAP */
 
 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 27)) && defined(CONFIG_HAS_WAKELOCK)
 	struct wake_lock 	wakelock[WAKE_LOCK_MAX];
@@ -423,6 +430,7 @@ 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(struct dhd_pub *dhd_pub, int ifidx);
 extern int wl_host_event(dhd_pub_t *dhd_pub, int *idx, void *pktdata,
                          wl_event_msg_t *, void **data_ptr);
diff --git a/drivers/net/wireless/bcmdhd/dhd_bus.h b/drivers/net/wireless/bcmdhd/dhd_bus.h
index 294322026e9..bccb8b6603f 100644
--- a/drivers/net/wireless/bcmdhd/dhd_bus.h
+++ b/drivers/net/wireless/bcmdhd/dhd_bus.h
@@ -48,6 +48,11 @@ 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);
 
diff --git a/drivers/net/wireless/bcmdhd/dhd_cdc.c b/drivers/net/wireless/bcmdhd/dhd_cdc.c
index 91c461f7bbe..0f9893a1537 100644
--- a/drivers/net/wireless/bcmdhd/dhd_cdc.c
+++ b/drivers/net/wireless/bcmdhd/dhd_cdc.c
@@ -916,7 +916,10 @@ _dhd_wlfc_send_signalonly_packet(athost_wl_status_info_t* ctx, wlfc_mac_descript
 #ifdef PROP_TXSTATUS_DEBUG
 		ctx->stats.signal_only_pkts_sent++;
 #endif
-		dhd_bus_txdata(((dhd_pub_t *)ctx->dhdp)->bus, p);
+		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",
@@ -1920,7 +1923,6 @@ dhd_wlfc_init(dhd_pub_t *dhd)
 		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,
@@ -2237,49 +2239,6 @@ dhd_prot_dstats(dhd_pub_t *dhd)
 	return;
 }
 
-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;
-
-#define htod32(i) i
-
-	if (dhd && dhd->up) {
-		if (value) {
-			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 */
-			enable_parm.id = htod32(100);
-			enable_parm.enable = htod32(1);
-			bcm_mkiovar("pkt_filter_enable", (char *)&enable_parm,
-				sizeof(wl_pkt_filter_enable_t), iovbuf, sizeof(iovbuf));
-			dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-			/* set bcn_li_dtim */
-			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);
-		} else {
-			power_mode = PM_FAST;
-			dhd_wl_ioctl_cmd(dhd, WLC_SET_PM, (char *)&power_mode,
-				sizeof(power_mode), TRUE, 0);
-			/* disable pkt filter */
-			enable_parm.id = htod32(100);
-			enable_parm.enable = htod32(0);
-			bcm_mkiovar("pkt_filter_enable", (char *)&enable_parm,
-				sizeof(wl_pkt_filter_enable_t), iovbuf, sizeof(iovbuf));
-			dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-			/* set bcn_li_dtim */
-			bcn_li_dtim = 0;
-			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);
-		}
-	}
-
-	return 0;
-}
 
 int
 dhd_prot_init(dhd_pub_t *dhd)
@@ -2297,16 +2256,17 @@ dhd_prot_init(dhd_pub_t *dhd)
 		goto done;
 
 
+#ifdef PROP_TXSTATUS
+	ret = dhd_wlfc_init(dhd);
+#endif
+
+#ifndef WL_CFG80211
 	ret = dhd_preinit_ioctls(dhd);
+#endif /* WL_CFG80211 */
 
 	/* Always assumes wl for now */
 	dhd->iswl = TRUE;
 
-#ifdef PROP_TXSTATUS
-	ret = dhd_wlfc_init(dhd);
-#endif
-	goto done;
-
 done:
 	return ret;
 }
diff --git a/drivers/net/wireless/bcmdhd/dhd_common.c b/drivers/net/wireless/bcmdhd/dhd_common.c
index c50f05325f6..f6bb8e5bb56 100644
--- a/drivers/net/wireless/bcmdhd/dhd_common.c
+++ b/drivers/net/wireless/bcmdhd/dhd_common.c
@@ -70,6 +70,7 @@ char nv_path[MOD_PARAM_PATHLEN];
 
 #ifdef SOFTAP
 char fw_path2[MOD_PARAM_PATHLEN];
+extern bool softap_enabled;
 #endif
 
 /* Last connection success/failure status */
@@ -1533,8 +1534,10 @@ dhd_preinit_ioctls(dhd_pub_t *dhd)
 	uint32 dongle_align = DHD_SDALIGN;
 	uint32 glom = 0;
 	uint bcn_timeout = 4;
+	uint retry_max = 3;
+#if defined(ARP_OFFLOAD_SUPPORT)
 	int arpoe = 1;
-	int arp_ol = 0xf;
+#endif
 	int scan_assoc_time = 40;
 	int scan_unassoc_time = 40;
 	const char 				*str;
@@ -1551,14 +1554,17 @@ dhd_preinit_ioctls(dhd_pub_t *dhd)
 #if defined(SOFTAP)
 	uint dtim = 1;
 #endif
-#ifdef AP
+#if (defined(AP) && !defined(WLP2P)) || (!defined(AP) && defined(WL_CFG80211))
 	uint32 mpc = 0; /* Turn MPC off for AP/APSTA mode */
+#endif /* AP */
+#if defined(AP) || defined(WLP2P)
 	uint32 apsta = 1; /* Enable APSTA mode */
-#endif
+#endif /* defined(AP) || defined(WLP2P) */
 #ifdef GET_CUSTOM_MAC_ENABLE
 	struct ether_addr ea_addr;
 #endif /* GET_CUSTOM_MAC_ENABLE */
 
+	dhd->op_mode = 0;
 #ifdef GET_CUSTOM_MAC_ENABLE
 	ret = dhd_custom_get_mac_address(ea_addr.octet);
 	if (!ret) {
@@ -1586,7 +1592,7 @@ dhd_preinit_ioctls(dhd_pub_t *dhd)
 #endif /* GET_CUSTOM_MAC_ENABLE */
 
 #ifdef SET_RANDOM_MAC_SOFTAP
-	if (strstr(fw_path, "apsta") != NULL) {
+	if (strstr(fw_path, "_apsta") != NULL) {
 		uint rand_mac;
 
 		srandom32((uint)jiffies);
@@ -1607,9 +1613,52 @@ dhd_preinit_ioctls(dhd_pub_t *dhd)
 	}
 #endif /* SET_RANDOM_MAC_SOFTAP */
 
-	DHD_ERROR(("Firmware up: Broadcom Dongle Host Driver mac=%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
-	       dhd->mac.octet[0], dhd->mac.octet[1], dhd->mac.octet[2],
-	       dhd->mac.octet[3], dhd->mac.octet[4], dhd->mac.octet[5]));
+	DHD_TRACE(("Firmware = %s\n", fw_path));
+#if !defined(AP) && defined(WLP2P)
+	/* Check if firmware with WFD support used */
+	if (strstr(fw_path, "_p2p") != NULL) {
+		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 for WFD failed ret= %d\n", __FUNCTION__, ret));
+		} else {
+			dhd->op_mode |= WFD_MASK;
+#if defined(ARP_OFFLOAD_SUPPORT)
+			arpoe = 0;
+#endif /* (ARP_OFFLOAD_SUPPORT) */
+			dhd_pkt_filter_enable = FALSE;
+		}
+	}
+#endif /* !defined(AP) && defined(WLP2P) */
+
+#if !defined(AP) && defined(WL_CFG80211)
+	/* Check if firmware with HostAPD support used */
+	if (strstr(fw_path, "_apsta") != NULL) {
+			/* 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) */
+				dhd_pkt_filter_enable = FALSE;
+			}
+	}
+#endif /* !defined(AP) && defined(WL_CFG80211) */
+
+	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;
+	}
+	DHD_ERROR(("Firmware up: op_mode=%d, "
+			"Broadcom Dongle Host Driver mac=%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
+			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) {
@@ -1649,15 +1698,16 @@ dhd_preinit_ioctls(dhd_pub_t *dhd)
 	/* 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);
-#ifdef AP
+	/* 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);
-
-	/* Enable APSTA mode */
 	bcm_mkiovar("apsta", (char *)&apsta, 4, iovbuf, sizeof(iovbuf));
 	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-#endif
+#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);
@@ -1720,12 +1770,6 @@ dhd_preinit_ioctls(dhd_pub_t *dhd)
 	dhd_wl_ioctl_cmd(dhd, WLC_SET_SCAN_UNASSOC_TIME, (char *)&scan_unassoc_time,
 		sizeof(scan_unassoc_time), TRUE, 0);
 
-	/* Set ARP offload */
-	bcm_mkiovar("arpoe", (char *)&arpoe, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-	bcm_mkiovar("arp_ol", (char *)&arp_ol, 4, iovbuf, sizeof(iovbuf));
-	dhd_wl_ioctl_cmd(dhd, WLC_SET_VAR, iovbuf, sizeof(iovbuf), TRUE, 0);
-
 	/* add a default packet filter pattern */
 	str = "pkt_filter_add";
 	str_len = strlen(str);
@@ -1779,9 +1823,9 @@ dhd_preinit_ioctls(dhd_pub_t *dhd)
 
 #ifdef ARP_OFFLOAD_SUPPORT
 	/* Set and enable ARP offload feature for STA only  */
-	if (dhd_arp_enable && !ap_fw_loaded) {
+	if (arpoe && !ap_fw_loaded) {
 		dhd_arp_offload_set(dhd, dhd_arp_mode);
-		dhd_arp_offload_enable(dhd, dhd_arp_enable);
+		dhd_arp_offload_enable(dhd, arpoe);
 	} else {
 		dhd_arp_offload_set(dhd, 0);
 		dhd_arp_offload_enable(dhd, FALSE);
@@ -2061,6 +2105,17 @@ 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
+	if (((dhd->op_mode & HOSTAPD_MASK) == HOSTAPD_MASK) ||
+		((dhd->op_mode & WFD_MASK) == WFD_MASK))
+		return TRUE;
+	else
+#endif /* WL_CFG80211 */
+		return FALSE;
+}
 
 #ifdef PNO_SUPPORT
 int
@@ -2105,6 +2160,8 @@ dhd_pno_enable(dhd_pub_t *dhd, int pfn_enabled)
 		return ret;
 	}
 
+	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
+		return (ret);
 	memset(iovbuf, 0, sizeof(iovbuf));
 	/* Check if disassoc to enable pno */
 	if ((pfn_enabled) &&
@@ -2152,6 +2209,8 @@ dhd_pno_set(dhd_pub_t *dhd, wlc_ssid_t* ssids_local, int nssid, ushort scan_fr,
 		DHD_ERROR(("%s error exit\n", __FUNCTION__));
 		err = -1;
 	}
+	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
+		return (err);
 
 	/* Check for broadcast ssid */
 	for (k = 0; k < nssid; k++) {
@@ -2268,7 +2327,10 @@ int dhd_keep_alive_onoff(dhd_pub_t *dhd)
 	int			str_len;
 	int res			= -1;
 
-	DHD_ERROR(("%s Enter\n", __FUNCTION__));
+	if (dhd_check_ap_wfd_mode_set(dhd) == TRUE)
+		return (res);
+
+	DHD_TRACE(("%s execution\n", __FUNCTION__));
 
 	str = "mkeep_alive";
 	str_len = strlen(str);
diff --git a/drivers/net/wireless/bcmdhd/dhd_linux.c b/drivers/net/wireless/bcmdhd/dhd_linux.c
index 5d35f78cf2a..03ba34a2c82 100644
--- a/drivers/net/wireless/bcmdhd/dhd_linux.c
+++ b/drivers/net/wireless/bcmdhd/dhd_linux.c
@@ -443,11 +443,9 @@ static void dhd_htsf_addrxts(dhd_pub_t *dhdp, void *pktbuf);
 static void dhd_dump_htsfhisto(histo_t *his, char *s);
 #endif /* WLMEDIA_HTSF */
 
-extern s32 wl_cfg80211_ifdel_ops(struct net_device *net);
-
 /* Monitor interface */
-extern int dhd_monitor_init(void *dhd_pub);
-extern int dhd_monitor_uninit(void);
+int dhd_monitor_init(void *dhd_pub);
+int dhd_monitor_uninit(void);
 
 
 #if defined(CONFIG_WIRELESS_EXT)
@@ -673,7 +671,7 @@ dhd_timeout_expired(dhd_timeout_t *tmo)
 	return 0;
 }
 
-static int
+int
 dhd_net2idx(dhd_info_t *dhd, struct net_device *net)
 {
 	int i = 0;
@@ -971,7 +969,8 @@ dhd_op_if(dhd_if_t *ifp)
 			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, dhd_net_attach)) {
+				if (!wl_cfg80211_notify_ifadd(ifp->net, ifp->idx, ifp->bssidx,
+					dhd_net_attach)) {
 					ifp->state = 0;
 					return;
 			}
@@ -1023,10 +1022,10 @@ dhd_op_if(dhd_if_t *ifp)
 			free_netdev(ifp->net);
 		}
 		dhd->iflist[ifp->idx] = NULL;
-		MFREE(dhd->pub.osh, ifp, sizeof(*ifp));
 #ifdef WL_CFG80211
-		if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211)
+		if (dhd->dhd_state & DHD_ATTACH_STATE_CFG80211) {
 			wl_cfg80211_notify_ifdel(ifp->net);
+		}
 #endif
 #ifdef SOFTAP
 		flags = dhd_os_spin_lock(&dhd->pub);
@@ -1034,6 +1033,7 @@ dhd_op_if(dhd_if_t *ifp)
 			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));
 	}
 }
 
@@ -1385,7 +1385,7 @@ dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *pktbuf, int numpkt, uint8 chan)
 	struct sk_buff *skb;
 	uchar *eth;
 	uint len;
-	void *data, *pnext, *save_pktbuf;
+	void *data, *pnext = NULL, *save_pktbuf;
 	int i;
 	dhd_if_t *ifp;
 	wl_event_msg_t event;
@@ -1398,6 +1398,16 @@ dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *pktbuf, int numpkt, uint8 chan)
 		struct ether_header *eh;
 		struct dot11_llc_snap_header *lsh;
 
+		ifp = dhd->iflist[ifidx];
+
+		/* Dropping packets before registering net device to avoid kernel panic */
+		if (!ifp->net || ifp->net->reg_state != NETREG_REGISTERED) {
+			DHD_ERROR(("%s: net device is NOT registered yet. drop packet\n",
+			__FUNCTION__));
+			PKTFREE(dhdp->osh, pktbuf, TRUE);
+			continue;
+		}
+
 		pnext = PKTNEXT(dhdp->osh, pktbuf);
 		PKTSETNEXT(wl->sh.osh, pktbuf, NULL);
 
@@ -1487,14 +1497,6 @@ dhd_rx_frame(dhd_pub_t *dhdp, int ifidx, void *pktbuf, int numpkt, uint8 chan)
 		dhdp->dstats.rx_bytes += skb->len;
 		dhdp->rx_packets++; /* Local count */
 
-		/* Dropping packets before registering net device to avoid kernel panic */
-		if (!ifp->net || ifp->net->reg_state != NETREG_REGISTERED) {
-			DHD_ERROR(("%s: net device is NOT registered yet. drop [%s] packet\n",
-			__FUNCTION__, (ntoh16(skb->protocol) == ETHER_TYPE_BRCM) ? "event" : "data"));
-			PKTFREE(dhdp->osh, pktbuf, TRUE);
-			continue;
-		}
-
 		if (in_interrupt()) {
 			netif_rx(skb);
 		} else {
@@ -2355,6 +2357,7 @@ dhd_add_if(dhd_info_t *dhd, int ifidx, void *handle, char *name,
 	if (handle == NULL) {
 		ifp->state = WLC_E_IF_ADD;
 		ifp->idx = ifidx;
+		ifp->bssidx = bssidx;
 		ASSERT(&dhd->thr_sysioc_ctl.thr_pid >= 0);
 		up(&dhd->thr_sysioc_ctl.sema);
 	} else
@@ -3057,6 +3060,7 @@ void dhd_detach(dhd_pub_t *dhdp)
 {
 	dhd_info_t *dhd;
 	unsigned long flags;
+	int timer_valid = FALSE;
 
 	if (!dhdp)
 		return;
@@ -3118,17 +3122,23 @@ void dhd_detach(dhd_pub_t *dhdp)
 		if (ifp->net->netdev_ops == &dhd_ops_pri)
 #endif
 		{
-			unregister_netdev(ifp->net);
+			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);
-	del_timer_sync(&dhd->timer);
+	if (timer_valid)
+		del_timer_sync(&dhd->timer);
 
 	if (dhd->dhd_state & DHD_ATTACH_STATE_THREADS_CREATED) {
 #ifdef DHDTHREAD
diff --git a/drivers/net/wireless/bcmdhd/dhd_linux_mon.c b/drivers/net/wireless/bcmdhd/dhd_linux_mon.c
index 6c1ff4d8ad4..dd9c71f75be 100644
--- a/drivers/net/wireless/bcmdhd/dhd_linux_mon.c
+++ b/drivers/net/wireless/bcmdhd/dhd_linux_mon.c
@@ -106,8 +106,10 @@ static struct net_device* lookup_real_netdev(char *name)
 	for (i = 0; i < DHD_MAX_IFS; i++) {
 		ndev = dhd_idx2net(g_monitor.dhd_pub, i);
 		if (ndev && strstr(name, ndev->name)) {
-			if (strlen(ndev->name) > last_name_len)
+			if (strlen(ndev->name) > last_name_len) {
 				ndev_found = ndev;
+				last_name_len = strlen(ndev->name);
+			}
 		}
 	}
 
diff --git a/drivers/net/wireless/bcmdhd/dhd_sdio.c b/drivers/net/wireless/bcmdhd/dhd_sdio.c
index db763ab8c3d..6d89f6b984a 100644
--- a/drivers/net/wireless/bcmdhd/dhd_sdio.c
+++ b/drivers/net/wireless/bcmdhd/dhd_sdio.c
@@ -52,6 +52,7 @@
 #include <sbsdio.h>
 #include <sbsdpcmdev.h>
 #include <bcmsdpcm.h>
+#include <bcmsdbus.h>
 
 #include <proto/ethernet.h>
 #include <proto/802.1d.h>
@@ -76,11 +77,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 */
 
@@ -360,7 +357,7 @@ static const uint retry_limit = 2;
 static bool forcealign;
 
 /* Flag to indicate if we should download firmware on driver load */
-uint dhd_download_fw_on_driverload = TRUE;
+uint dhd_download_fw_on_driverload = FALSE;
 
 #define ALIGNMENT  4
 
@@ -1026,7 +1023,9 @@ dhdsdio_txpkt(dhd_bus_t *bus, void *pkt, uint chan, bool free_pkt)
 	htol32_ua_store(0, frame + SDPCM_FRAMETAG_LEN + sizeof(swheader));
 
 #ifdef DHD_DEBUG
-	tx_packets[PKTPRIO(pkt)]++;
+	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))))) {
@@ -1843,6 +1842,8 @@ dhdsdio_readshared(dhd_bus_t *bus, sdpcm_shared_t *sh)
 	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",
@@ -6219,10 +6220,13 @@ dhd_bus_devreset(dhd_pub_t *dhdp, uint8 flag)
 
 			dhd_os_sdunlock(dhdp);
 		} else {
-			bcmerror = BCME_NOTDOWN;
-			DHD_ERROR(("%s: Set DEVRESET=FALSE invoked when device is on\n",
+			DHD_INFO(("%s called when dongle is not in reset\n",
 				__FUNCTION__));
-			bcmerror = BCME_SDIO_ERROR;
+			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;
diff --git a/drivers/net/wireless/bcmdhd/include/bcmutils.h b/drivers/net/wireless/bcmdhd/include/bcmutils.h
index f7f23009fdd..530036f0ba7 100644
--- a/drivers/net/wireless/bcmdhd/include/bcmutils.h
+++ b/drivers/net/wireless/bcmdhd/include/bcmutils.h
@@ -140,7 +140,9 @@ typedef bool (*ifpkt_cb_t)(void*, int);
 #define SHARED_POOL		((struct pktpool *)NULL)
 #endif 
 
+#ifndef PKTPOOL_LEN_MAX
 #define PKTPOOL_LEN_MAX		40
+#endif
 #define PKTPOOL_CB_MAX		3
 
 struct pktpool;
diff --git a/drivers/net/wireless/bcmdhd/include/epivers.h b/drivers/net/wireless/bcmdhd/include/epivers.h
index 5c63a391321..f474dfa482a 100644
--- a/drivers/net/wireless/bcmdhd/include/epivers.h
+++ b/drivers/net/wireless/bcmdhd/include/epivers.h
@@ -33,17 +33,17 @@
 
 #define	EPI_RC_NUMBER		125
 
-#define	EPI_INCREMENTAL_NUMBER	52
+#define	EPI_INCREMENTAL_NUMBER	69
 
 #define	EPI_BUILD_NUMBER	0
 
-#define	EPI_VERSION		5, 90, 125, 52
+#define	EPI_VERSION		5, 90, 125, 69
 
-#define	EPI_VERSION_NUM		0x055a7d34
+#define	EPI_VERSION_NUM		0x055a7d45
 
 #define EPI_VERSION_DEV		5.90.125
 
 
-#define	EPI_VERSION_STR		"5.90.125.52"
+#define	EPI_VERSION_STR		"5.90.125.69"
 
 #endif 
diff --git a/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h b/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h
index 7f51faa0d3d..30ec848c40a 100644
--- a/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h
+++ b/drivers/net/wireless/bcmdhd/include/proto/bcmevent.h
@@ -181,7 +181,8 @@ typedef BWL_PRE_PACKED_STRUCT struct bcm_event {
 #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_LAST		84	
+#define WLC_E_GTK_PLUMBED 84
+#define WLC_E_LAST	85
 
 
 typedef struct {
diff --git a/drivers/net/wireless/bcmdhd/include/sdio.h b/drivers/net/wireless/bcmdhd/include/sdio.h
index 7e94e7df13c..ca932266a1b 100644
--- a/drivers/net/wireless/bcmdhd/include/sdio.h
+++ b/drivers/net/wireless/bcmdhd/include/sdio.h
@@ -376,6 +376,7 @@ typedef volatile struct {
 #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
@@ -411,6 +412,10 @@ typedef volatile struct {
 
 #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
diff --git a/drivers/net/wireless/bcmdhd/include/wlioctl.h b/drivers/net/wireless/bcmdhd/include/wlioctl.h
index a441eabc076..1059de147ef 100644
--- a/drivers/net/wireless/bcmdhd/include/wlioctl.h
+++ b/drivers/net/wireless/bcmdhd/include/wlioctl.h
@@ -190,6 +190,7 @@ typedef struct wlc_ssid {
 #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;    
@@ -554,6 +555,7 @@ typedef enum sup_auth_status {
 #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
@@ -615,6 +617,9 @@ typedef struct {
 #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
@@ -1498,6 +1503,7 @@ typedef struct wl_sampledata {
 #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 
diff --git a/drivers/net/wireless/bcmdhd/wl_android.c b/drivers/net/wireless/bcmdhd/wl_android.c
index e054d7b3886..d6471d99106 100644
--- a/drivers/net/wireless/bcmdhd/wl_android.c
+++ b/drivers/net/wireless/bcmdhd/wl_android.c
@@ -338,10 +338,10 @@ int wl_android_wifi_off(struct net_device *dev)
 	dhd_net_if_lock(dev);
 	if (g_wifi_on) {
 		dhd_dev_reset(dev, 1);
-		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
 		sdioh_stop(NULL);
 		/* clean up dtim_skip setting */
 		net_os_set_dtim_skip(dev, TRUE);
+		dhd_customer_gpio_wlan_ctrl(WLAN_RESET_OFF);
 		g_wifi_on = 0;
 	}
 	dhd_net_if_unlock(dev);
diff --git a/drivers/net/wireless/bcmdhd/wl_cfg80211.c b/drivers/net/wireless/bcmdhd/wl_cfg80211.c
index 366ce2224ab..09529136787 100644
--- a/drivers/net/wireless/bcmdhd/wl_cfg80211.c
+++ b/drivers/net/wireless/bcmdhd/wl_cfg80211.c
@@ -2,13 +2,13 @@
  * 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
@@ -74,7 +74,7 @@
 #include <wl_cfgp2p.h>
 
 static struct sdio_func *cfg80211_sdio_func;
-static struct wl_dev *wl_cfg80211_dev;
+static struct wl_priv *wlcfg_drv_priv;
 
 u32 wl_dbg_level = WL_DBG_ERR;
 
@@ -83,7 +83,7 @@ u32 wl_dbg_level = WL_DBG_ERR;
 #define WL_TRACE(a) printk("%s ", __FUNCTION__); printk a
 #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 3000
+#define MAX_WAIT_TIME 1500
 static s8 ioctlbuf[WLC_IOCTL_MAXLEN];
 
 #if defined(DHD_P2P_DEV_ADDR_FROM_SYSFS) && defined(CONFIG_SYSCTL)
@@ -148,13 +148,10 @@ static const struct ieee80211_regdomain brcm_regdom = {
 		NL80211_RRF_PASSIVE_SCAN |
 		NL80211_RRF_NO_IBSS |
 		NL80211_RRF_NO_OFDM),
-		/* IEEE 802.11a, channel 36..48 */
-		REG_RULE(5180-10, 5240+10, 40, 6, 20, 0),
-
-		/* NB: 5260 MHz - 5700 MHz requies DFS */
-
-		/* IEEE 802.11a, channel 149..165 */
-		REG_RULE(5745-10, 5825+10, 40, 6, 20, 0), }
+		/* 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), }
 };
 
 
@@ -339,8 +336,7 @@ static u32 wl_get_ielen(struct wl_priv *wl);
 
 static s32 wl_mode_to_nl80211_iftype(s32 mode);
 
-static struct wireless_dev *wl_alloc_wdev(s32 sizeof_iface,
-	struct device *dev);
+static struct wireless_dev *wl_alloc_wdev(struct device *sdiofunc_dev);
 static void wl_free_wdev(struct wl_priv *wl);
 
 static s32 wl_inform_bss(struct wl_priv *wl);
@@ -365,12 +361,6 @@ static void wl_deinit_priv_mem(struct wl_priv *wl);
 static void wl_delay(u32 ms);
 
 /*
- * store/restore cfg80211 instance data
- */
-static void wl_set_drvdata(struct wl_dev *dev, void *data);
-static void *wl_get_drvdata(struct wl_dev *dev);
-
-/*
  * ibss mode utilities
  */
 static bool wl_is_ibssmode(struct wl_priv *wl, struct net_device *ndev);
@@ -423,9 +413,9 @@ 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 wlc_ssid *ssid,
+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);
+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,
@@ -474,23 +464,11 @@ 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 WL_PRIV_GET() 							\
-	({								\
-	struct wl_iface *ci = NULL;					\
-	if (unlikely(!(wl_cfg80211_dev && 				\
-		(ci = wl_get_drvdata(wl_cfg80211_dev))))) {		\
-		WL_ERR(("wl_cfg80211_dev is unavailable\n"));		\
-		BUG();							\
-	} 								\
-	ci_to_wl(ci);							\
-})
-
-#define CHECK_SYS_UP()							\
+#define CHECK_SYS_UP(wlpriv)							\
 do {									\
-	struct wl_priv *wl = WL_PRIV_GET();			\
-	if (unlikely(!wl_get_drv_status(wl, READY))) {	\
+	if (unlikely(!wl_get_drv_status(wlpriv, READY))) {	\
 		WL_INFO(("device is not ready : status (%d)\n",		\
-			(int)wl->status));				\
+			(int)wlpriv->status));			\
 		return -EIO;						\
 	}								\
 } while (0)
@@ -609,70 +587,7 @@ static struct ieee80211_channel __wl_5ghz_a_channels[] = {
 	CHAN5G(132, 0), CHAN5G(136, 0),
 	CHAN5G(140, 0), CHAN5G(149, 0),
 	CHAN5G(153, 0), CHAN5G(157, 0),
-	CHAN5G(161, 0), CHAN5G(165, 0),
-	CHAN5G(184, 0), CHAN5G(188, 0),
-	CHAN5G(192, 0), CHAN5G(196, 0),
-	CHAN5G(200, 0), CHAN5G(204, 0),
-	CHAN5G(208, 0), CHAN5G(212, 0),
-	CHAN5G(216, 0)
-};
-
-static struct ieee80211_channel __wl_5ghz_n_channels[] = {
-	CHAN5G(32, 0), 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(50, 0),
-	CHAN5G(52, 0), CHAN5G(54, 0),
-	CHAN5G(56, 0), CHAN5G(58, 0),
-	CHAN5G(60, 0), CHAN5G(62, 0),
-	CHAN5G(64, 0), CHAN5G(66, 0),
-	CHAN5G(68, 0), CHAN5G(70, 0),
-	CHAN5G(72, 0), CHAN5G(74, 0),
-	CHAN5G(76, 0), CHAN5G(78, 0),
-	CHAN5G(80, 0), CHAN5G(82, 0),
-	CHAN5G(84, 0), CHAN5G(86, 0),
-	CHAN5G(88, 0), CHAN5G(90, 0),
-	CHAN5G(92, 0), CHAN5G(94, 0),
-	CHAN5G(96, 0), CHAN5G(98, 0),
-	CHAN5G(100, 0), CHAN5G(102, 0),
-	CHAN5G(104, 0), CHAN5G(106, 0),
-	CHAN5G(108, 0), CHAN5G(110, 0),
-	CHAN5G(112, 0), CHAN5G(114, 0),
-	CHAN5G(116, 0), CHAN5G(118, 0),
-	CHAN5G(120, 0), CHAN5G(122, 0),
-	CHAN5G(124, 0), CHAN5G(126, 0),
-	CHAN5G(128, 0), CHAN5G(130, 0),
-	CHAN5G(132, 0), CHAN5G(134, 0),
-	CHAN5G(136, 0), CHAN5G(138, 0),
-	CHAN5G(140, 0), CHAN5G(142, 0),
-	CHAN5G(144, 0), CHAN5G(145, 0),
-	CHAN5G(146, 0), CHAN5G(147, 0),
-	CHAN5G(148, 0), CHAN5G(149, 0),
-	CHAN5G(150, 0), CHAN5G(151, 0),
-	CHAN5G(152, 0), CHAN5G(153, 0),
-	CHAN5G(154, 0), CHAN5G(155, 0),
-	CHAN5G(156, 0), CHAN5G(157, 0),
-	CHAN5G(158, 0), CHAN5G(159, 0),
-	CHAN5G(160, 0), CHAN5G(161, 0),
-	CHAN5G(162, 0), CHAN5G(163, 0),
-	CHAN5G(164, 0), CHAN5G(165, 0),
-	CHAN5G(166, 0), CHAN5G(168, 0),
-	CHAN5G(170, 0), CHAN5G(172, 0),
-	CHAN5G(174, 0), CHAN5G(176, 0),
-	CHAN5G(178, 0), CHAN5G(180, 0),
-	CHAN5G(182, 0), CHAN5G(184, 0),
-	CHAN5G(186, 0), CHAN5G(188, 0),
-	CHAN5G(190, 0), CHAN5G(192, 0),
-	CHAN5G(194, 0), CHAN5G(196, 0),
-	CHAN5G(198, 0), CHAN5G(200, 0),
-	CHAN5G(202, 0), CHAN5G(204, 0),
-	CHAN5G(206, 0), CHAN5G(208, 0),
-	CHAN5G(210, 0), CHAN5G(212, 0),
-	CHAN5G(214, 0), CHAN5G(216, 0),
-	CHAN5G(218, 0), CHAN5G(220, 0),
-	CHAN5G(222, 0), CHAN5G(224, 0),
-	CHAN5G(226, 0), CHAN5G(228, 0)
+	CHAN5G(161, 0), CHAN5G(165, 0)
 };
 
 static struct ieee80211_supported_band __wl_band_2ghz = {
@@ -691,14 +606,6 @@ static struct ieee80211_supported_band __wl_band_5ghz_a = {
 	.n_bitrates = wl_a_rates_size
 };
 
-static struct ieee80211_supported_band __wl_band_5ghz_n = {
-	.band = IEEE80211_BAND_5GHZ,
-	.channels = __wl_5ghz_n_channels,
-	.n_channels = ARRAY_SIZE(__wl_5ghz_n_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,
@@ -876,11 +783,10 @@ wl_cfg80211_add_virtual_iface(struct wiphy *wiphy, char *name,
 	s32 index = 0;
 	s32 mode = 0;
 	chanspec_t chspec;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	struct net_device *_ndev;
 	dhd_pub_t *dhd = (dhd_pub_t *)(wl->pub);
 	int (*net_attach)(dhd_pub_t *dhdp, int ifidx);
-
 	WL_DBG(("if name: %s, type: %d\n", name, type));
 	switch (type) {
 	case NL80211_IFTYPE_ADHOC:
@@ -968,7 +874,8 @@ wl_cfg80211_add_virtual_iface(struct wiphy *wiphy, char *name,
 				return ERR_PTR(-ENOMEM);
 			}
 			vwdev->wiphy = wl->wdev->wiphy;
-			WL_INFO((" virtual interface(%s) is created \n", wl->p2p->vir_ifname));
+			WL_INFO((" virtual interface(%s) is created memalloc done \n",
+			wl->p2p->vir_ifname));
 			index = alloc_idx_vwdev(wl);
 			wl->vwdev[index] = vwdev;
 			vwdev->iftype =
@@ -981,12 +888,11 @@ wl_cfg80211_add_virtual_iface(struct wiphy *wiphy, char *name,
 			wl_set_drv_status(wl, READY);
 			wl->p2p->vif_created = true;
 			set_mode_by_netdev(wl, _ndev, mode);
-			wl = wdev_to_wl(vwdev);
 			net_attach =  wl_to_p2p_bss_private(wl, P2PAPI_BSSCFG_CONNECTION);
 			rtnl_unlock();
 			if (net_attach && !net_attach(dhd, _ndev->ifindex))
 				WL_DBG((" virtual interface(%s) is "
-					"created\n", wl->p2p->vir_ifname));
+					"created net attach done\n", wl->p2p->vir_ifname));
 			else {
 				rtnl_lock();
 				goto fail;
@@ -1010,10 +916,10 @@ static s32
 wl_cfg80211_del_virtual_iface(struct wiphy *wiphy, struct net_device *dev)
 {
 	struct ether_addr p2p_mac;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	s32 timeout = -1;
 	s32 ret = 0;
-
+	WL_DBG(("Enter\n"));
 	if (wl->p2p_supported) {
 		memcpy(p2p_mac.octet, wl->p2p->int_addr.octet, ETHER_ADDR_LEN);
 		if (wl->p2p->vif_created) {
@@ -1021,8 +927,17 @@ wl_cfg80211_del_virtual_iface(struct wiphy *wiphy, struct net_device *dev)
 				wl_cfg80211_scan_abort(wl, dev);
 			}
 
-			wl_cfgp2p_ifdel(wl, &p2p_mac);
+			ret = wl_cfgp2p_ifdel(wl, &p2p_mac);
 			wl_set_p2p_status(wl, IF_DELETING);
+			if (ret) {
+				/* 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
+				*/
+				dhd_pub_t *dhd = (dhd_pub_t *)(wl->pub);
+				WL_ERR(("Firmware returned an error from p2p_ifdel, try to remove linux virtual network interface dev->name %s\n", dev->name));
+				dhd_del_if(dhd->info, dhd_net2idx(dhd->info, dev));
+			}
 
 			/* Wait for any pending scan req to get aborted from the sysioc context */
 			timeout = wait_event_interruptible_timeout(wl->dongle_event_wait,
@@ -1052,7 +967,7 @@ wl_cfg80211_change_virtual_iface(struct wiphy *wiphy, struct net_device *ndev,
 	s32 wlif_type;
 	s32 mode = 0;
 	chanspec_t chspec;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	WL_DBG(("Enter \n"));
 	switch (type) {
 	case NL80211_IFTYPE_MONITOR:
@@ -1118,10 +1033,10 @@ wl_cfg80211_change_virtual_iface(struct wiphy *wiphy, struct net_device *ndev,
 }
 
 s32
-wl_cfg80211_notify_ifadd(struct net_device *net, s32 idx,
+wl_cfg80211_notify_ifadd(struct net_device *net, s32 idx, s32 bssidx,
 int (*_net_attach)(dhd_pub_t *dhdp, int ifidx))
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	s32 ret = BCME_OK;
 	if (!net) {
 		WL_ERR(("net is NULL\n"));
@@ -1133,11 +1048,11 @@ int (*_net_attach)(dhd_pub_t *dhdp, int ifidx))
 		/* Assign the net device to CONNECT BSSCFG */
 		strncpy(net->name, wl->p2p->vir_ifname, IFNAMSIZ - 1);
 		wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION) = net;
-		wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_CONNECTION) =
-			P2PAPI_BSSCFG_CONNECTION;
+		wl_to_p2p_bss_bssidx(wl, P2PAPI_BSSCFG_CONNECTION) = bssidx;
 		wl_to_p2p_bss_private(wl, P2PAPI_BSSCFG_CONNECTION) = _net_attach;
-		wl_clr_p2p_status(wl, IF_ADD);
 		net->ifindex = idx;
+		wl_clr_p2p_status(wl, IF_ADD);
+
 		wake_up_interruptible(&wl->dongle_event_wait);
 	}
 	return ret;
@@ -1146,7 +1061,7 @@ int (*_net_attach)(dhd_pub_t *dhdp, int ifidx))
 s32
 wl_cfg80211_ifdel_ops(struct net_device *net)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 
 	if (!net || !net->name) {
 		WL_DBG(("net is NULL\n"));
@@ -1172,14 +1087,14 @@ wl_cfg80211_ifdel_ops(struct net_device *net)
 s32
 wl_cfg80211_notify_ifdel(struct net_device *net)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 
 
 	if (wl->p2p->vif_created) {
 		s32 index = 0;
 
-		WL_DBG(("IF_DEL event called from dongle, _net name: %s, vif name: %s\n",
-			net->name, wl->p2p->vir_ifname));
+		WL_DBG(("IF_DEL event called from dongle, net %x, vif name: %s\n",
+			(unsigned int)net, wl->p2p->vir_ifname));
 
 		memset(wl->p2p->vir_ifname, '\0', IFNAMSIZ);
 		index = wl_cfgp2p_find_idx(wl, net);
@@ -1208,7 +1123,7 @@ s32
 wl_cfg80211_is_progress_ifadd(void)
 {
 	s32 is_progress = 0;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	if (wl_get_p2p_status(wl, IF_ADD))
 		is_progress = 1;
 	return is_progress;
@@ -1218,7 +1133,7 @@ s32
 wl_cfg80211_is_progress_ifchange(void)
 {
 	s32 is_progress = 0;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	if (wl_get_p2p_status(wl, IF_CHANGING))
 		is_progress = 1;
 	return is_progress;
@@ -1228,7 +1143,7 @@ wl_cfg80211_is_progress_ifchange(void)
 s32
 wl_cfg80211_notify_ifchange(void)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	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->dongle_event_wait);
@@ -1236,8 +1151,16 @@ wl_cfg80211_notify_ifchange(void)
 	return 0;
 }
 
-static void wl_iscan_prep(struct wl_scan_params *params, struct wlc_ssid *ssid)
+static void wl_scan_prep(struct wl_scan_params *params, struct cfg80211_scan_request *request)
 {
+	u32 n_ssids = request->n_ssids;
+	u32 n_channels = request->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;
@@ -1246,63 +1169,142 @@ static void wl_iscan_prep(struct wl_scan_params *params, struct wlc_ssid *ssid)
 	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 (ssid && ssid->SSID_len)
-		memcpy(&params->ssid, ssid, sizeof(wlc_ssid_t));
 
+	/* 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 wlc_ssid *ssid, u16 action)
+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));
+	    (WL_SCAN_PARAMS_FIXED_SIZE + offsetof(wl_iscan_params_t, params));
 	struct wl_iscan_params *params;
 	s32 err = 0;
 
-	if (ssid && ssid->SSID_len)
-		params_size += sizeof(struct wlc_ssid);
+	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 (unlikely(!params))
-		return -ENOMEM;
-	memset(params, 0, params_size);
-	BUG_ON(unlikely(params_size >= WLC_IOCTL_SMLEN));
+	if (!params) {
+		err = -ENOMEM;
+		goto done;
+	}
 
-	wl_iscan_prep(&params->params, ssid);
+	if (request != NULL)
+		wl_scan_prep(&params->params, request);
 
 	params->version = htod32(ISCAN_REQ_VERSION);
 	params->action = htod16(action);
 	params->scan_duration = htod16(0);
 
-	/* params_size += offsetof(wl_iscan_params_t, params); */
+	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_SMLEN);
+		iscan->ioctl_buf, WLC_IOCTL_MEDLEN);
 	if (unlikely(err)) {
 		if (err == -EBUSY) {
-		WL_INFO(("system busy : iscan canceled\n"));
+			WL_INFO(("system busy : iscan canceled\n"));
 		} else {
 			WL_ERR(("error (%d)\n", err));
 		}
 	}
 	kfree(params);
+done:
 	return err;
 }
 
-static s32 wl_do_iscan(struct wl_priv *wl)
+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);
-	struct wlc_ssid ssid;
 	s32 passive_scan;
 	s32 err = 0;
 
-	/* Broadcast scan by default */
-	memset(&ssid, 0, sizeof(ssid));
-
 	iscan->state = WL_ISCAN_STATE_SCANING;
 
 	passive_scan = wl->active_scan ? 0 : 1;
@@ -1313,7 +1315,7 @@ static s32 wl_do_iscan(struct wl_priv *wl)
 		return err;
 	}
 	wl->iscan_kickstart = true;
-	wl_run_iscan(iscan, &ssid, WL_SCAN_ACTION_START);
+	wl_run_iscan(iscan, request, WL_SCAN_ACTION_START);
 	mod_timer(&iscan->timer, jiffies + iscan->timer_ms * HZ / 1000);
 	iscan->timer_on = 1;
 
@@ -1321,9 +1323,12 @@ static s32 wl_do_iscan(struct wl_priv *wl)
 }
 
 static s32
-wl_run_escan(struct wl_priv *wl, struct net_device *ndev, wlc_ssid_t *ssid, uint16 action)
+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;
 	struct cfg80211_scan_request *scan_request = wl->scan_request;
@@ -1337,31 +1342,37 @@ wl_run_escan(struct wl_priv *wl, struct net_device *ndev, wlc_ssid_t *ssid, uint
 	if (!wl->p2p_supported || ((ndev == wl_to_prmry_ndev(wl)) &&
 		!p2p_scan(wl))) {
 		/* LEGACY SCAN TRIGGER */
-		WL_DBG(("LEGACY SCAN START\n"));
-		if (ssid && ssid->SSID_len) {
-			params_size += sizeof(wlc_ssid_t);
+		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 *) kmalloc(params_size, GFP_KERNEL);
-
-		if (params == NULL)
-			return -ENOMEM;
-
-		memset(params, 0, params_size);
-		memcpy(&params->params.bssid, &ether_bcast, ETHER_ADDR_LEN);
-		params->params.bss_type = DOT11_BSSTYPE_ANY;
-		params->params.scan_type = 0;
-		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 = 0;
-		if (ssid && ssid->SSID_len) {
-			memcpy(params->params.ssid.SSID, ssid->SSID, ssid->SSID_len);
-			params->params.ssid.SSID_len = htod32(ssid->SSID_len);
+		params = (wl_escan_params_t *) kzalloc(params_size, GFP_KERNEL);
+		if (params == NULL) {
+			err = -ENOMEM;
+			goto exit;
 		}
+
+		if (request != NULL)
+			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"));
+			err = -ENOMEM;
+			goto exit;
+		}
 		wldev_iovar_setbuf(ndev, "escan", params, params_size,
 			wl->escan_ioctl_buf, WLC_IOCTL_MEDLEN);
 		kfree(params);
@@ -1370,7 +1381,7 @@ wl_run_escan(struct wl_priv *wl, struct net_device *ndev, wlc_ssid_t *ssid, uint
 		/* P2P SCAN TRIGGER */
 		if (scan_request && scan_request->n_channels) {
 			num_chans = scan_request->n_channels;
-			WL_INFO((" chann number : %d\n", num_chans));
+			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) {
@@ -1407,12 +1418,13 @@ exit:
 
 
 static s32
-wl_do_escan(struct wl_priv *wl, struct wiphy *wiphy, struct net_device *ndev, wlc_ssid_t *ssid)
+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_DBG(("Enter \n"));
+	WL_SCAN(("Enter \n"));
 
 	wl->escan_info.wiphy = wiphy;
 	wl->escan_info.escan_state = WL_ESCAN_STATE_SCANING;
@@ -1428,7 +1440,7 @@ wl_do_escan(struct wl_priv *wl, struct wiphy *wiphy, struct net_device *ndev, wl
 	results->count = 0;
 	results->buflen = WL_SCAN_RESULTS_FIXED_SIZE;
 
-	wl_run_escan(wl, ndev, ssid, WL_SCAN_ACTION_START);
+	wl_run_escan(wl, ndev, request, WL_SCAN_ACTION_START);
 	return err;
 }
 
@@ -1437,15 +1449,16 @@ __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 	struct cfg80211_scan_request *request,
 	struct cfg80211_ssid *this_ssid)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	struct cfg80211_ssid *ssids;
 	struct wl_scan_req *sr = wl_to_sr(wl);
-	wlc_ssid_t ssid_info;
 	s32 passive_scan;
 	bool iscan_req;
 	bool escan_req;
 	bool spec_scan;
+	bool p2p_ssid;
 	s32 err = 0;
+	s32 i;
 
 	if (unlikely(wl_get_drv_status(wl, SCANNING))) {
 		WL_ERR(("Scanning already : status (%d)\n", (int)wl->status));
@@ -1456,6 +1469,10 @@ __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 			(int)wl->status));
 		return -EAGAIN;
 	}
+	if (request->n_ssids > WL_SCAN_PARAMS_SSID_MAX) {
+		WL_ERR(("n_ssids > WL_SCAN_PARAMS_SSID_MAX\n"));
+		return -EOPNOTSUPP;
+	}
 
 	WL_DBG(("wiphy (%p)\n", wiphy));
 
@@ -1463,11 +1480,18 @@ __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 	spec_scan = false;
 	if (request) {		/* scan bss */
 		ssids = request->ssids;
-		if (wl->iscan_on && (!ssids || !ssids->ssid_len)) {
+		if (wl->iscan_on && (!ssids || !ssids->ssid_len || request->n_ssids != 1)) {
 			iscan_req = true;
 		} else if (wl->escan_on) {
 			escan_req = true;
-			if (ssids->ssid_len && IS_P2P_SSID(ssids->ssid)) {
+			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) {
@@ -1477,7 +1501,6 @@ __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 					}
 					p2p_scan(wl) = true;
 				}
-
 			} else {
 				/* legacy scan trigger
 				 * So, we have to disable p2p discovery if p2p discovery is on
@@ -1510,17 +1533,12 @@ __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 	wl->scan_request = request;
 	wl_set_drv_status(wl, SCANNING);
 	if (iscan_req) {
-		err = wl_do_iscan(wl);
+		err = wl_do_iscan(wl, request);
 		if (likely(!err))
 			return err;
 		else
 			goto scan_out;
 	} else if (escan_req) {
-		WL_DBG(("ssid \"%s\", ssid_len (%d)\n",
-			ssids->ssid, ssids->ssid_len));
-
-		memcpy(ssid_info.SSID, ssids->ssid, ssids->ssid_len);
-		ssid_info.SSID_len = ssids->ssid_len;
 		if (wl->p2p_supported) {
 			if (p2p_on(wl) && p2p_scan(wl)) {
 
@@ -1532,7 +1550,7 @@ __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 				}
 			}
 		}
-		err = wl_do_escan(wl, wiphy, ndev, &ssid_info);
+		err = wl_do_escan(wl, wiphy, ndev, request);
 		if (likely(!err))
 			return err;
 		else
@@ -1546,18 +1564,18 @@ __wl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
 		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_DBG(("Specific scan ssid=\"%s\" len=%d\n",
+			WL_SCAN(("Specific scan ssid=\"%s\" len=%d\n",
 				sr->ssid.SSID, sr->ssid.SSID_len));
 			spec_scan = true;
 		} else {
-			WL_DBG(("Broadcast scan\n"));
+			WL_SCAN(("Broadcast scan\n"));
 		}
-		WL_DBG(("sr->ssid.SSID_len (%d)\n", sr->ssid.SSID_len));
+		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_ERR(("WLC_SET_PASSIVE_SCAN error (%d)\n", err));
+			WL_SCAN(("WLC_SET_PASSIVE_SCAN error (%d)\n", err));
 			goto scan_out;
 		}
 		err = wldev_ioctl(ndev, WLC_SCAN, &sr->ssid,
@@ -1586,9 +1604,10 @@ 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();
+	CHECK_SYS_UP(wl);
 	err = __wl_cfg80211_scan(wiphy, ndev, request, NULL);
 	if (unlikely(err)) {
 		WL_ERR(("scan error (%d)\n", err));
@@ -1679,11 +1698,11 @@ static s32 wl_set_retry(struct net_device *dev, u32 retry, bool l)
 
 static s32 wl_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 {
-	struct wl_priv *wl = wiphy_to_wl(wiphy);
+	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();
+	CHECK_SYS_UP(wl);
 	if (changed & WIPHY_PARAM_RTS_THRESHOLD &&
 		(wl->conf->rts_threshold != wiphy->rts_threshold)) {
 		wl->conf->rts_threshold = wiphy->rts_threshold;
@@ -1721,7 +1740,7 @@ static s32
 wl_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	struct cfg80211_ibss_params *params)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	struct cfg80211_bss *bss;
 	struct ieee80211_channel *chan;
 	struct wl_join_params join_params;
@@ -1730,7 +1749,7 @@ wl_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	s32 err = 0;
 
 	WL_TRACE(("In\n"));
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 	if (params->bssid) {
 		WL_ERR(("Invalid bssid\n"));
 		return -EOPNOTSUPP;
@@ -1791,10 +1810,10 @@ wl_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 
 static s32 wl_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	s32 err = 0;
 
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 	wl_link_down(wl);
 
 	return err;
@@ -1803,7 +1822,7 @@ static s32 wl_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *dev)
 static s32
 wl_set_wpa_version(struct net_device *dev, struct cfg80211_connect_params *sme)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	struct wl_security *sec;
 	s32 val = 0;
 	s32 err = 0;
@@ -1833,7 +1852,7 @@ 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)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	struct wl_security *sec;
 	s32 val = 0;
 	s32 err = 0;
@@ -1872,7 +1891,7 @@ 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)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	struct wl_security *sec;
 	s32 pval = 0;
 	s32 gval = 0;
@@ -1944,7 +1963,7 @@ 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)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	struct wl_security *sec;
 	s32 val = 0;
 	s32 err = 0;
@@ -2002,7 +2021,7 @@ static s32
 wl_set_set_sharedkey(struct net_device *dev,
 	struct cfg80211_connect_params *sme)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	struct wl_security *sec;
 	struct wl_wsec_key key;
 	s32 val;
@@ -2068,14 +2087,17 @@ static s32
 wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 	struct cfg80211_connect_params *sme)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	struct ieee80211_channel *chan = sme->channel;
 	struct wl_join_params join_params;
 	size_t join_params_size;
 	s32 err = 0;
-
+	wpa_ie_fixed_t *wpa_ie;
+	bcm_tlv_t *wpa2_ie;
+	u8* wpaie  = 0;
+	u32 wpaie_len = 0;
 	WL_DBG(("In\n"));
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 
 	/*
 	 * Cancel ongoing scan to sync up with sme state machine of cfg80211.
@@ -2107,8 +2129,28 @@ wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 				VNDR_IE_ASSOCREQ_FLAG, sme->ie, sme->ie_len);
 		}
 
-	} else {
-		WL_INFO(("No P2PIE in beacon \n"));
+	} 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,
+				ioctlbuf, sizeof(ioctlbuf));
+			} else {
+				wldev_iovar_setbuf(dev, "wpaie", NULL, 0,
+				ioctlbuf, sizeof(ioctlbuf));
+			}
+
 	}
 
 	if (unlikely(!sme->ssid)) {
@@ -2119,7 +2161,8 @@ wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 		wl->channel = ieee80211_frequency_to_channel(chan->center_freq);
 		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))
@@ -2153,7 +2196,10 @@ wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 	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, NULL, &join_params.ssid, WL_PROF_SSID);
-	memcpy(&join_params.params.bssid, &ether_bcast, ETHER_ADDR_LEN);
+	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));
@@ -2162,13 +2208,13 @@ wl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 		WL_INFO(("ssid \"%s\", len (%d)\n", join_params.ssid.SSID,
 			join_params.ssid.SSID_len));
 	}
+	wl_set_drv_status(wl, CONNECTING);
 	err = wldev_ioctl(dev, WLC_SET_SSID, &join_params, join_params_size, false);
 	if (unlikely(err)) {
 		WL_ERR(("error (%d)\n", err));
+		wl_clr_drv_status(wl, CONNECTING);
 		return err;
 	}
-	wl_set_drv_status(wl, CONNECTING);
-
 	return err;
 }
 
@@ -2176,21 +2222,29 @@ static s32
 wl_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
 	u16 reason_code)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	scb_val_t scbval;
 	bool act = false;
 	s32 err = 0;
 
 	WL_ERR(("Reason %d\n\n\n", reason_code));
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 	act = *(bool *) wl_read_prof(wl, WL_PROF_ACT);
 	if (likely(act)) {
+		/*
+		* Cancel ongoing scan to sync up with sme state machine of cfg80211.
+		*/
+		if (wl->scan_request) {
+			wl_cfg80211_scan_abort(wl, dev);
+		}
+		wl_set_drv_status(wl, DISCONNECTING);
 		scbval.val = reason_code;
 		memcpy(&scbval.ea, &wl->bssid, ETHER_ADDR_LEN);
 		scbval.val = htod32(scbval.val);
 		err = wldev_ioctl(dev, WLC_DISASSOC, &scbval,
 			sizeof(scb_val_t), false);
 		if (unlikely(err)) {
+			wl_clr_drv_status(wl, DISCONNECTING);
 			WL_ERR(("error (%d)\n", err));
 			return err;
 		}
@@ -2204,13 +2258,13 @@ wl_cfg80211_set_tx_power(struct wiphy *wiphy,
 	enum nl80211_tx_power_setting type, s32 dbm)
 {
 
-	struct wl_priv *wl = WL_PRIV_GET();
+	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();
+	CHECK_SYS_UP(wl);
 	switch (type) {
 	case NL80211_TX_POWER_AUTOMATIC:
 		break;
@@ -2253,13 +2307,13 @@ wl_cfg80211_set_tx_power(struct wiphy *wiphy,
 
 static s32 wl_cfg80211_get_tx_power(struct wiphy *wiphy, s32 *dbm)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	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();
+	CHECK_SYS_UP(wl);
 	err = wl_dev_intvar_get(ndev, "qtxpower", &txpwrdbm);
 	if (unlikely(err)) {
 		WL_ERR(("error (%d)\n", err));
@@ -2275,14 +2329,14 @@ 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 = WL_PRIV_GET();
+	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();
+	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));
@@ -2305,7 +2359,7 @@ 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 = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	struct wl_wsec_key key;
 	s32 err = 0;
 	s32 bssidx = wl_cfgp2p_find_idx(wl, dev);
@@ -2404,10 +2458,10 @@ wl_cfg80211_add_key(struct wiphy *wiphy, struct net_device *dev,
 	s32 err = 0;
 	u8 keybuf[8];
 	s32 bssidx = 0;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	s32 mode = get_mode_by_netdev(wl, dev);
 	WL_DBG(("key index (%d)\n", key_idx));
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 
 	bssidx = wl_cfgp2p_find_idx(wl, dev);
 
@@ -2505,12 +2559,12 @@ 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 = WL_PRIV_GET();
+	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();
+	CHECK_SYS_UP(wl);
 	memset(&key, 0, sizeof(key));
 
 	key.index = (u32) key_idx;
@@ -2554,14 +2608,14 @@ wl_cfg80211_get_key(struct wiphy *wiphy, struct net_device *dev,
 {
 	struct key_params params;
 	struct wl_wsec_key key;
-	struct wl_priv *wl = WL_PRIV_GET();
+	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();
+	CHECK_SYS_UP(wl);
 	memset(&key, 0, sizeof(key));
 	key.index = key_idx;
 	swap_key_to_BE(&key);
@@ -2607,7 +2661,6 @@ wl_cfg80211_config_default_mgmt_key(struct wiphy *wiphy,
 	struct net_device *dev, u8 key_idx)
 {
 	WL_INFO(("Not supported\n"));
-	CHECK_SYS_UP();
 	return -EOPNOTSUPP;
 }
 
@@ -2615,13 +2668,13 @@ static s32
 wl_cfg80211_get_station(struct wiphy *wiphy, struct net_device *dev,
 	u8 *mac, struct station_info *sinfo)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	scb_val_t scb_val;
 	int rssi;
 	s32 rate;
 	s32 err = 0;
 
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 	if (unlikely
 		(memcmp(mac, wl_read_prof(wl, WL_PROF_BSSID), ETHER_ADDR_LEN))) {
 		WL_ERR(("Wrong Mac address\n"));
@@ -2672,9 +2725,15 @@ wl_cfg80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
 {
 	s32 pm;
 	s32 err = 0;
+	struct wl_priv *wl = wiphy_priv(wiphy);
 
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 	pm = enabled ? PM_FAST : PM_OFF;
+	/* Do not enable the power save after assoc if it is p2p interface */
+	if (wl->p2p && wl->p2p->vif_created) {
+		WL_DBG(("Do not enable the power save for p2p interfaces even after assoc\n"));
+		pm = PM_OFF;
+	}
 	pm = htod32(pm);
 	WL_DBG(("power save %s\n", (pm ? "enabled" : "disabled")));
 	err = wldev_ioctl(dev, WLC_SET_PM, &pm, sizeof(pm), false);
@@ -2717,7 +2776,7 @@ static __used u32 wl_find_msb(u16 bit16)
 
 static s32 wl_cfg80211_resume(struct wiphy *wiphy)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	s32 err = 0;
 
 	if (unlikely(!wl_get_drv_status(wl, READY))) {
@@ -2737,9 +2796,8 @@ static s32 wl_cfg80211_suspend(struct wiphy *wiphy, struct cfg80211_wowlan *wow)
 static s32 wl_cfg80211_suspend(struct wiphy *wiphy)
 #endif
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	s32 err = 0;
-
 	if (unlikely(!wl_get_drv_status(wl, READY))) {
 		WL_INFO(("device is not ready : status (%d)\n",
 			(int)wl->status));
@@ -2763,7 +2821,7 @@ wl_update_pmklist(struct net_device *dev, struct wl_pmk_list *pmk_list,
 	s32 err)
 {
 	int i, j;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	struct net_device *primary_dev = wl_to_prmry_ndev(wl);
 
 	/* Firmware is supporting pmk list only for STA interface i.e. primary interface
@@ -2771,7 +2829,7 @@ wl_update_pmklist(struct net_device *dev, struct wl_pmk_list *pmk_list,
 	  * Do we really need to support PMK cache in P2P in firmware?
 	*/
 	if (primary_dev != dev) {
-		WL_ERR(("Not supporting Flushing pmklist on virtual"
+		WL_INFO(("Not supporting Flushing pmklist on virtual"
 			" interfaces than primary interface\n"));
 		return err;
 	}
@@ -2796,11 +2854,11 @@ static s32
 wl_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *dev,
 	struct cfg80211_pmksa *pmksa)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	s32 err = 0;
 	int i;
 
-	CHECK_SYS_UP();
+	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))
@@ -2816,10 +2874,10 @@ wl_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *dev,
 		err = -EINVAL;
 	}
 	WL_DBG(("set_pmksa,IW_PMKSA_ADD - PMKID: %pM =\n",
-		&wl->pmk_list->pmkids.pmkid[wl->pmk_list->pmkids.npmkid].BSSID));
+		&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].
+			wl->pmk_list->pmkids.pmkid[wl->pmk_list->pmkids.npmkid - 1].
 			PMKID[i]));
 	}
 
@@ -2832,12 +2890,12 @@ static s32
 wl_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
 	struct cfg80211_pmksa *pmksa)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	struct _pmkid_list pmkid;
 	s32 err = 0;
 	int i;
 
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 	memcpy(&pmkid.pmkid[0].BSSID, pmksa->bssid, ETHER_ADDR_LEN);
 	memcpy(&pmkid.pmkid[0].PMKID, pmksa->pmkid, WPA2_PMKID_LEN);
 
@@ -2878,9 +2936,9 @@ wl_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *dev,
 static s32
 wl_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *dev)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	s32 err = 0;
-	CHECK_SYS_UP();
+	CHECK_SYS_UP(wl);
 	memset(wl->pmk_list, 0, sizeof(*wl->pmk_list));
 	err = wl_update_pmklist(dev, wl->pmk_list, err);
 	return err;
@@ -2956,7 +3014,7 @@ wl_cfg80211_remain_on_channel(struct wiphy *wiphy, struct net_device *dev,
 	s32 target_channel;
 
 	s32 err = BCME_OK;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	dhd_pub_t *dhd = (dhd_pub_t *)(wl->pub);
 	WL_DBG(("Enter, netdev_ifidx: %d \n", dev->ifindex));
 	if (likely(wl_get_drv_status(wl, SCANNING))) {
@@ -2976,12 +3034,12 @@ wl_cfg80211_remain_on_channel(struct wiphy *wiphy, struct net_device *dev,
 		 * without turning on P2P
 		 */
 
+		p2p_on(wl) = true;
 		err = wl_cfgp2p_enable_discovery(wl, dev, NULL, 0);
 
 		if (unlikely(err)) {
 			goto exit;
 		}
-		p2p_on(wl) = true;
 	}
 	if (p2p_on(wl))
 		wl_cfgp2p_discover_listen(wl, target_channel, duration);
@@ -3012,7 +3070,7 @@ wl_cfg80211_mgmt_tx(struct wiphy *wiphy, struct net_device *dev,
 	wifi_p2p_ie_t *p2p_ie;
 	wpa_ie_fixed_t *wps_ie;
 	const struct ieee80211_mgmt *mgmt;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wiphy_priv(wiphy);
 	dhd_pub_t *dhd = (dhd_pub_t *)(wl->pub);
 	s32 err = BCME_OK;
 	s32 bssidx = 0;
@@ -3467,7 +3525,7 @@ wl_cfg80211_set_beacon(struct wiphy *wiphy, struct net_device *dev,
 	s32 err = BCME_OK;
 	bcm_tlv_t *ssid_ie;
 	wlc_ssid_t ssid;
-	struct wl_priv *wl = WL_PRIV_GET();
+	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;
@@ -3582,7 +3640,10 @@ wl_cfg80211_set_beacon(struct wiphy *wiphy, struct net_device *dev,
 			wldev_iovar_setint(dev, "mpc", 0);
 			wldev_ioctl(dev, WLC_DOWN, &ap, sizeof(s32), false);
 			wldev_ioctl(dev, WLC_SET_INFRA, &infra, sizeof(s32), false);
-			wldev_ioctl(dev, WLC_SET_AP, &ap, sizeof(s32), false);
+			if ((err = wldev_ioctl(dev, WLC_SET_AP, &ap, sizeof(s32), false)) < 0) {
+				WL_ERR(("setting AP mode failed %d \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) {
@@ -3757,7 +3818,6 @@ wl_cfg80211_set_beacon(struct wiphy *wiphy, struct net_device *dev,
 static s32
 wl_cfg80211_drv_start(struct wiphy *wiphy, struct net_device *dev)
 {
-	/* struct wl_priv *wl = wiphy_to_wl(wiphy); */
 	s32 err = 0;
 
 	printk("Android driver start command\n");
@@ -3767,7 +3827,6 @@ wl_cfg80211_drv_start(struct wiphy *wiphy, struct net_device *dev)
 static s32
 wl_cfg80211_drv_stop(struct wiphy *wiphy, struct net_device *dev)
 {
-	/* struct wl_priv *wl = wiphy_to_wl(wiphy); */
 	s32 err = 0;
 
 	printk("Android driver stop command\n");
@@ -3830,28 +3889,26 @@ static s32 wl_mode_to_nl80211_iftype(s32 mode)
 	return err;
 }
 
-static struct wireless_dev *wl_alloc_wdev(s32 sizeof_iface,
-	struct device *dev)
+static struct wireless_dev *wl_alloc_wdev(struct device *sdiofunc_dev)
 {
 	struct wireless_dev *wdev;
 	s32 err = 0;
-	struct wl_priv *wl;
 	wdev = kzalloc(sizeof(*wdev), GFP_KERNEL);
 	if (unlikely(!wdev)) {
 		WL_ERR(("Could not allocate wireless device\n"));
 		return ERR_PTR(-ENOMEM);
 	}
 	wdev->wiphy =
-	    wiphy_new(&wl_cfg80211_ops, sizeof(struct wl_priv) + sizeof_iface);
+	    wiphy_new(&wl_cfg80211_ops, sizeof(struct wl_priv));
 	if (unlikely(!wdev->wiphy)) {
 		WL_ERR(("Couldn not allocate wiphy device\n"));
 		err = -ENOMEM;
 		goto wiphy_new_out;
 	}
-	set_wiphy_dev(wdev->wiphy, dev);
-	wl = wiphy_to_wl(wdev->wiphy);
+	set_wiphy_dev(wdev->wiphy, sdiofunc_dev);
 	wdev->wiphy->max_scan_ie_len = WL_SCAN_IE_LEN_MAX;
-	wdev->wiphy->max_scan_ssids = WL_NUM_SCAN_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;
 	wdev->wiphy->interface_modes =
 	    BIT(NL80211_IFTYPE_STATION) | BIT(NL80211_IFTYPE_ADHOC)
@@ -3899,7 +3956,7 @@ wiphy_new_out:
 static void wl_free_wdev(struct wl_priv *wl)
 {
 	int i;
-	struct wireless_dev *wdev = wl_to_wdev(wl);
+	struct wireless_dev *wdev = wl->wdev;
 
 	if (unlikely(!wdev)) {
 		WL_ERR(("wdev is invalid\n"));
@@ -3913,9 +3970,9 @@ static void wl_free_wdev(struct wl_priv *wl)
 		}
 	}
 	wiphy_unregister(wdev->wiphy);
+	wdev->wiphy->dev.parent = NULL;
 	wiphy_free(wdev->wiphy);
 	kfree(wdev);
-	wl_to_wdev(wl) = NULL;
 }
 
 static s32 wl_inform_bss(struct wl_priv *wl)
@@ -3926,13 +3983,6 @@ static s32 wl_inform_bss(struct wl_priv *wl)
 	s32 i;
 
 	bss_list = wl->bss_list;
-#if 0
-	if (unlikely(bss_list->version != WL_BSS_INFO_VERSION)) {
-		WL_ERR(("Version %d != WL_BSS_INFO_VERSION\n",
-			bss_list->version));
-		return -EOPNOTSUPP;
-	}
-#endif
 	WL_DBG(("scanned AP count (%d)\n", bss_list->count));
 	bi = next_bss(bss_list, bi);
 	for_each_bss(bss_list, bi, i) {
@@ -4165,33 +4215,42 @@ wl_notify_connect_status(struct wl_priv *wl, struct net_device *ndev,
 			ntoh32(e->event_type), ntoh32(e->status)));
 		if (wl_is_linkup(wl, e, ndev)) {
 			wl_link_up(wl);
+			act = true;
+			wl_update_prof(wl, e, &act, WL_PROF_ACT);
 			if (wl_is_ibssmode(wl, ndev)) {
 				printk("cfg80211_ibss_joined");
 				cfg80211_ibss_joined(ndev, (s8 *)&e->addr,
 					GFP_KERNEL);
 				WL_DBG(("joined in IBSS network\n"));
 			} else {
-				printk("wl_bss_connect_done succeeded");
-				wl_bss_connect_done(wl, ndev, e, data, true);
-				WL_DBG(("joined in BSS network \"%s\"\n",
+				if (!wl_get_drv_status(wl, DISCONNECTING)) {
+					printk("wl_bss_connect_done succeeded");
+					wl_bss_connect_done(wl, ndev, e, data, true);
+					WL_DBG(("joined in BSS network \"%s\"\n",
 					((struct wlc_ssid *)
 					 wl_read_prof(wl, WL_PROF_SSID))->SSID));
+				}
 			}
-			act = true;
-			wl_update_prof(wl, e, &act, WL_PROF_ACT);
+
 		} else if (wl_is_linkdown(wl, e)) {
+			if (wl->scan_request) {
+				if (wl->escan_on) {
+					wl_notify_escan_complete(wl, true);
+				} else
+					wl_iscan_aborted(wl);
+			}
 			if (wl_get_drv_status(wl, CONNECTED)) {
 				printk("link down, call cfg80211_disconnected ");
-				rtnl_lock();
-				cfg80211_disconnected(ndev, 0, NULL, 0, GFP_KERNEL);
 				wl_clr_drv_status(wl, CONNECTED);
+				cfg80211_disconnected(ndev, 0, NULL, 0, GFP_KERNEL);
 				wl_link_down(wl);
 				wl_init_prof(wl->profile);
-				rtnl_unlock();
 			} else if (wl_get_drv_status(wl, CONNECTING)) {
 				printk("link down, during connecting");
 				wl_bss_connect_done(wl, ndev, e, data, false);
 			}
+			wl_clr_drv_status(wl, DISCONNECTING);
+
 		} else if (wl_is_nonetwork(wl, e)) {
 			printk("connect failed e->status 0x%x", (int)ntoh32(e->status));
 			if (wl_get_drv_status(wl, CONNECTING))
@@ -4215,24 +4274,22 @@ wl_notify_roaming_status(struct wl_priv *wl, struct net_device *ndev,
 	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 (test_bit(WL_STATUS_CONNECTED, &wl->status))
+		if (wl_get_drv_status(wl, CONNECTED))
 			wl_bss_roaming_done(wl, ndev, e, data);
 		else
 			wl_bss_connect_done(wl, ndev, e, data, true);
 		act = true;
 		wl_update_prof(wl, e, &act, WL_PROF_ACT);
 	}
-
 	return err;
 }
 
 static __used s32
 wl_dev_bufvar_set(struct net_device *dev, s8 *name, s8 *buf, s32 len)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	u32 buflen;
 
 	buflen = bcm_mkiovar(name, buf, len, wl->ioctl_buf, WL_IOCTL_LEN_MAX);
@@ -4245,7 +4302,7 @@ static s32
 wl_dev_bufvar_get(struct net_device *dev, s8 *name, s8 *buf,
 	s32 buf_len)
 {
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 	u32 len;
 	s32 err = 0;
 
@@ -4279,6 +4336,14 @@ static s32 wl_get_assoc_ies(struct wl_priv *wl, struct net_device *ndev)
 	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 = wl_dev_bufvar_get(ndev, "assoc_req_ies", wl->extra_buf,
 			WL_ASSOC_INFO_MAX);
@@ -4290,11 +4355,15 @@ static s32 wl_get_assoc_ies(struct wl_priv *wl, struct net_device *ndev)
 		if (assoc_info.flags & WLC_ASSOC_REQ_IS_REASSOC) {
 			conn_info->req_ie_len -= ETHER_ADDR_LEN;
 		}
-		conn_info->req_ie =
-		    kmemdup(wl->extra_buf, conn_info->req_ie_len, GFP_KERNEL);
+		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;
-		conn_info->req_ie = NULL;
 	}
 	if (assoc_info.resp_len) {
 		err = wl_dev_bufvar_get(ndev, "assoc_resp_ies", wl->extra_buf,
@@ -4304,11 +4373,15 @@ static s32 wl_get_assoc_ies(struct wl_priv *wl, struct net_device *ndev)
 			return err;
 		}
 		conn_info->resp_ie_len = assoc_info.resp_len -sizeof(struct dot11_assoc_resp);
-		conn_info->resp_ie =
-		    kmemdup(wl->extra_buf, conn_info->resp_ie_len, GFP_KERNEL);
+		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;
-		conn_info->resp_ie = NULL;
 	}
 	WL_DBG(("req len (%d) resp len (%d)\n", conn_info->req_ie_len,
 		conn_info->resp_ie_len));
@@ -4437,6 +4510,7 @@ wl_bss_roaming_done(struct wl_priv *wl, struct net_device *ndev,
 	wl_get_assoc_ies(wl, ndev);
 	memcpy(&wl->bssid, &e->addr, ETHER_ADDR_LEN);
 	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,
@@ -4459,11 +4533,18 @@ wl_bss_connect_done(struct wl_priv *wl, struct net_device *ndev,
 	s32 err = 0;
 
 	WL_DBG((" enter\n"));
-	wl_get_assoc_ies(wl, ndev);
-	memcpy(&wl->bssid, &e->addr, ETHER_ADDR_LEN);
-	wl_update_bss_info(wl, ndev);
+	if (wl->scan_request) {
+			wl_cfg80211_scan_abort(wl, ndev);
+	}
 	if (wl_get_drv_status(wl, CONNECTING)) {
 		wl_clr_drv_status(wl, CONNECTING);
+		if (completed) {
+			wl_get_assoc_ies(wl, ndev);
+			memcpy(&wl->bssid, &e->addr, ETHER_ADDR_LEN);
+			wl_update_bss_info(wl, ndev);
+			wl_update_pmklist(ndev, wl->pmk_list, err);
+			wl_set_drv_status(wl, CONNECTED);
+		}
 		cfg80211_connect_result(ndev,
 			(u8 *)&wl->bssid,
 			conn_info->req_ie,
@@ -4475,14 +4556,6 @@ wl_bss_connect_done(struct wl_priv *wl, struct net_device *ndev,
 		WL_DBG(("Report connect result - connection %s\n",
 			completed ? "succeeded" : "failed"));
 	}
-	if (completed)
-		wl_set_drv_status(wl, CONNECTED);
-	else {
-		if (wl->scan_request) {
-			wl_cfg80211_scan_abort(wl, ndev);
-		}
-	}
-
 	return err;
 }
 
@@ -4740,7 +4813,7 @@ static s32 wl_init_priv_mem(struct wl_priv *wl)
 		WL_ERR(("Ioctl buf alloc failed\n"));
 		goto init_priv_mem_out;
 	}
-	wl->escan_ioctl_buf = (void *)kzalloc(WL_IOCTL_LEN_MAX, GFP_KERNEL);
+	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;
@@ -4809,24 +4882,20 @@ static void wl_deinit_priv_mem(struct wl_priv *wl)
 
 static s32 wl_create_event_handler(struct wl_priv *wl)
 {
+	int ret = 0;
 	WL_DBG(("Enter \n"));
-	sema_init(&wl->event_sync, 0);
-	wl->event_tsk = kthread_run(wl_event_handler, wl, "wl_event_handler");
-	if (IS_ERR(wl->event_tsk)) {
-		wl->event_tsk = NULL;
-		WL_ERR(("failed to create event thread\n"));
-		return -ENOMEM;
-	}
-	return 0;
+
+	wl->event_tsk.thr_pid = DHD_PID_KT_INVALID;
+	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) {
-		send_sig(SIGTERM, wl->event_tsk, 1);
-		kthread_stop(wl->event_tsk);
-		wl->event_tsk = NULL;
-	}
+	if (wl->event_tsk.thr_pid >= 0)
+		PROC_STOP(&wl->event_tsk);
 }
 
 static void wl_term_iscan(struct wl_priv *wl)
@@ -5046,6 +5115,7 @@ static void wl_notify_escan_complete(struct wl_priv *wl, bool aborted)
 	if (unlikely(!wl_get_drv_status(wl, SCANNING))) {
 		wl_clr_drv_status(wl, SCANNING);
 		WL_ERR(("Scan complete while device not scanning\n"));
+		wl->scan_request = NULL;
 		return;
 	}
 	wl_clr_drv_status(wl, SCANNING);
@@ -5152,7 +5222,9 @@ static s32 wl_escan_handler(struct wl_priv *wl,
 		wl->escan_info.escan_state = WL_ESCAN_STATE_IDLE;
 		if (likely(wl->scan_request)) {
 			rtnl_lock();
-			WL_INFO(("ESCAN COMPLETED\n"));
+			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, true);
 			rtnl_unlock();
 		}
@@ -5251,7 +5323,7 @@ s32 wl_cfg80211_sysctl_export_devaddr(void *data)
 	 * so that wpa_supplicant can access it
 	 */
 	dhd_pub_t *dhd = (dhd_pub_t *)data;
-	struct wl_priv *wl = WL_PRIV_GET();
+	struct wl_priv *wl = wlcfg_drv_priv;
 
 	wl_cfgp2p_generate_bss_mac(&dhd->mac, &wl->p2p->dev_addr, &wl->p2p->int_addr);
 
@@ -5271,7 +5343,7 @@ s32 wl_cfg80211_attach_post(struct net_device *ndev)
 		WL_ERR(("ndev is invaild\n"));
 		return -ENODEV;
 	}
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 	if (wl && !wl_get_drv_status(wl, READY)) {
 			if (wl->wdev &&
 				wl_cfgp2p_supported(wl, ndev)) {
@@ -5296,7 +5368,6 @@ s32 wl_cfg80211_attach(struct net_device *ndev, void *data)
 {
 	struct wireless_dev *wdev;
 	struct wl_priv *wl;
-	struct wl_iface *ci;
 	s32 err = 0;
 
 	WL_TRACE(("In\n"));
@@ -5304,23 +5375,16 @@ s32 wl_cfg80211_attach(struct net_device *ndev, void *data)
 		WL_ERR(("ndev is invaild\n"));
 		return -ENODEV;
 	}
-	wl_cfg80211_dev = kzalloc(sizeof(struct wl_dev), GFP_KERNEL);
-	if (unlikely(!wl_cfg80211_dev)) {
-		WL_ERR(("wl_cfg80211_dev is invalid\n"));
-		return -ENOMEM;
-	}
 	WL_DBG(("func %p\n", wl_cfg80211_get_sdio_func()));
-	wdev = wl_alloc_wdev(sizeof(struct wl_iface), &wl_cfg80211_get_sdio_func()->dev);
+	wdev = wl_alloc_wdev(&wl_cfg80211_get_sdio_func()->dev);
 	if (unlikely(IS_ERR(wdev)))
 		return -ENOMEM;
 
 	wdev->iftype = wl_mode_to_nl80211_iftype(WL_MODE_BSS);
-	wl = wdev_to_wl(wdev);
+	wl = (struct wl_priv *)wiphy_priv(wdev->wiphy);
 	wl->wdev = wdev;
 	wl->pub = data;
 
-	ci = (struct wl_iface *)wl_to_ci(wl);
-	ci->wl = wl;
 	ndev->ieee80211_ptr = wdev;
 	SET_NETDEV_DEV(ndev, wiphy_dev(wdev->wiphy));
 	wdev->netdev = ndev;
@@ -5343,7 +5407,7 @@ s32 wl_cfg80211_attach(struct net_device *ndev, void *data)
 		goto cfg80211_attach_out;
 	}
 #endif
-	wl_set_drvdata(wl_cfg80211_dev, ci);
+	wlcfg_drv_priv = wl;
 	return err;
 
 cfg80211_attach_out:
@@ -5356,7 +5420,7 @@ void wl_cfg80211_detach(void)
 {
 	struct wl_priv *wl;
 
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 
 	WL_TRACE(("In\n"));
 #if defined(DHD_P2P_DEV_ADDR_FROM_SYSFS) && defined(CONFIG_SYSCTL)
@@ -5367,29 +5431,30 @@ void wl_cfg80211_detach(void)
 	if (wl->p2p_supported)
 		wl_cfgp2p_deinit_priv(wl);
 	wl_deinit_priv(wl);
-	wl_free_wdev(wl);
-	wl_set_drvdata(wl_cfg80211_dev, NULL);
-	kfree(wl_cfg80211_dev);
-	wl_cfg80211_dev = NULL;
+	wlcfg_drv_priv = NULL;
 	wl_clear_sdio_func();
+	wl_free_wdev(wl);
 }
 
 static void wl_wakeup_event(struct wl_priv *wl)
 {
-	up(&wl->event_sync);
+	if (wl->event_tsk.thr_pid >= 0)
+		up(&wl->event_tsk.sema);
 }
 
 static s32 wl_event_handler(void *data)
 {
 	struct net_device *netdev;
-	struct wl_priv *wl = (struct wl_priv *)data;
-	struct sched_param param = {.sched_priority = MAX_RT_PRIO - 1 };
+	struct wl_priv *wl = NULL;
 	struct wl_event_q *e;
+	tsk_ctl_t *tsk = (tsk_ctl_t *)data;
 
-	sched_setscheduler(current, SCHED_FIFO, &param);
-	allow_signal(SIGTERM);
-	while (likely(!down_interruptible(&wl->event_sync))) {
-		if (kthread_should_stop())
+	wl = (struct wl_priv *)tsk->parent;
+	complete(&tsk->completed);
+
+	while (down_interruptible (&tsk->sema) == 0) {
+		SMP_RD_BARRIER_DEPENDS();
+		if (tsk->terminated)
 			break;
 		e = wl_deq_event(wl);
 		if (unlikely(!e)) {
@@ -5400,7 +5465,7 @@ static s32 wl_event_handler(void *data)
 		netdev = dhd_idx2net((struct dhd_pub *)(wl->pub), e->emsg.ifidx);
 		if (!netdev)
 			netdev = wl_to_prmry_ndev(wl);
-		if (wl->evt_handler[e->etype]) {
+		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));
@@ -5408,6 +5473,7 @@ static s32 wl_event_handler(void *data)
 		wl_put_event(e);
 	}
 	WL_DBG(("%s was terminated\n", __func__));
+	complete_and_exit(&tsk->completed, 0);
 	return 0;
 }
 
@@ -5415,7 +5481,7 @@ 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 = WL_PRIV_GET();
+	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) ?
@@ -5619,14 +5685,12 @@ static s32 wl_dongle_eventmsg(struct net_device *ndev)
 	setbit(eventmask, WLC_E_DISASSOC_IND);
 	setbit(eventmask, WLC_E_DISASSOC);
 	setbit(eventmask, WLC_E_JOIN);
+	setbit(eventmask, WLC_E_ROAM);
 	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_PMKID_CACHE);
 	setbit(eventmask, WLC_E_TXFAIL);
-	setbit(eventmask, WLC_E_JOIN_START);
 	setbit(eventmask, WLC_E_SCAN_COMPLETE);
 	setbit(eventmask, WLC_E_ACTION_FRAME_RX);
 	setbit(eventmask, WLC_E_ACTION_FRAME_COMPLETE);
@@ -5997,27 +6061,24 @@ default_conf_out:
 static s32 wl_update_wiphybands(struct wl_priv *wl)
 {
 	struct wiphy *wiphy;
-	s32 phy_list;
-	s8 phy;
+	s8 phylist_buf[128];
+	s8 *phy;
 	s32 err = 0;
 
-	err = wldev_ioctl(wl_to_prmry_ndev(wl), WLC_GET_PHYLIST, &phy_list,
-		sizeof(phy_list), false);
+	err = wldev_ioctl(wl_to_prmry_ndev(wl), WLC_GET_PHYLIST, phylist_buf,
+		sizeof(phylist_buf), false);
 	if (unlikely(err)) {
 		WL_ERR(("error (%d)\n", err));
 		return err;
 	}
-
-	phy = ((char *)&phy_list)[1];
-	WL_DBG(("%c phy\n", phy));
-	if (phy == 'a') {
-		wiphy = wl_to_wiphy(wl);
-		wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_a;
-	} else if (phy == 'n') {
-		wiphy = wl_to_wiphy(wl);
-		wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_n;
+	phy = phylist_buf;
+	for (; *phy; phy++) {
+		if (*phy == 'a' || *phy == 'n') {
+			wiphy = wl_to_wiphy(wl);
+			wiphy->bands[IEEE80211_BAND_5GHZ] =
+				&__wl_band_5ghz_a;
+		}
 	}
-
 	return err;
 }
 
@@ -6056,7 +6117,9 @@ static s32 __wl_cfg80211_down(struct wl_priv *wl)
 	wl_clr_drv_status(wl, READY);
 	wl_clr_drv_status(wl, SCANNING);
 	wl_clr_drv_status(wl, SCAN_ABORTING);
+	wl_clr_drv_status(wl, CONNECTING);
 	wl_clr_drv_status(wl, CONNECTED);
+	wl_clr_drv_status(wl, DISCONNECTING);
 	if (wl_get_drv_status(wl, AP_CREATED)) {
 		wl_clr_drv_status(wl, AP_CREATED);
 		wl_clr_drv_status(wl, AP_CREATING);
@@ -6081,7 +6144,7 @@ s32 wl_cfg80211_up(void)
 	s32 err = 0;
 
 	WL_TRACE(("In\n"));
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 	mutex_lock(&wl->usr_sync);
 	wl_cfg80211_attach_post(wl_to_prmry_ndev(wl));
 	err = __wl_cfg80211_up(wl);
@@ -6098,7 +6161,7 @@ s32 wl_cfg80211_down(void)
 	s32 err = 0;
 
 	WL_TRACE(("In\n"));
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 	mutex_lock(&wl->usr_sync);
 	err = __wl_cfg80211_down(wl);
 	mutex_unlock(&wl->usr_sync);
@@ -6266,11 +6329,7 @@ static void wl_link_down(struct wl_priv *wl)
 
 	WL_DBG(("In\n"));
 	wl->link_up = false;
-	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;
 }
 
@@ -6299,22 +6358,12 @@ static void wl_delay(u32 ms)
 	}
 }
 
-static void wl_set_drvdata(struct wl_dev *dev, void *data)
-{
-	dev->driver_data = data;
-}
-
-static void *wl_get_drvdata(struct wl_dev *dev)
-{
-	return dev->driver_data;
-}
-
 s32 wl_cfg80211_read_fw(s8 *buf, u32 size)
 {
 	const struct firmware *fw_entry;
 	struct wl_priv *wl;
 
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 
 	fw_entry = wl->fw->fw_entry;
 
@@ -6330,7 +6379,7 @@ void wl_cfg80211_release_fw(void)
 {
 	struct wl_priv *wl;
 
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 	release_firmware(wl->fw->fw_entry);
 	wl->fw->ptr = 0;
 }
@@ -6343,7 +6392,7 @@ void *wl_cfg80211_request_fw(s8 *file_name)
 
 	WL_TRACE(("In\n"));
 	WL_DBG(("file name : \"%s\"\n", file_name));
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 
 	if (!test_bit(WL_FW_LOADING_DONE, &wl->fw->status)) {
 		err = request_firmware(&wl->fw->fw_entry, file_name,
@@ -6388,7 +6437,7 @@ s8 *wl_cfg80211_get_fwname(void)
 {
 	struct wl_priv *wl;
 
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 	strcpy(wl->fw->fw_name, WL_4329_FW_FILE);
 	return wl->fw->fw_name;
 }
@@ -6397,7 +6446,7 @@ s8 *wl_cfg80211_get_nvramname(void)
 {
 	struct wl_priv *wl;
 
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 	strcpy(wl->fw->nvram_name, WL_4329_NVRAM_FILE);
 	return wl->fw->nvram_name;
 }
@@ -6408,7 +6457,7 @@ s32 wl_cfg80211_get_p2p_dev_addr(struct net_device *net, struct ether_addr *p2pd
 	dhd_pub_t *dhd_pub;
 	struct ether_addr p2pif_addr;
 
-	wl = WL_PRIV_GET();
+	wl = wlcfg_drv_priv;
 	dhd_pub = (dhd_pub_t *)wl->pub;
 	wl_cfgp2p_generate_bss_mac(&dhd_pub->mac, p2pdev_addr, &p2pif_addr);
 
diff --git a/drivers/net/wireless/bcmdhd/wl_cfg80211.h b/drivers/net/wireless/bcmdhd/wl_cfg80211.h
index 838003bd644..a5637240c17 100644
--- a/drivers/net/wireless/bcmdhd/wl_cfg80211.h
+++ b/drivers/net/wireless/bcmdhd/wl_cfg80211.h
@@ -52,6 +52,7 @@ struct wl_ibss;
 #define dtohchanspec(i) i
 
 #define WL_DBG_NONE	0
+#define WL_DBG_SCAN 	(1 << 3)
 #define WL_DBG_DBG 	(1 << 2)
 #define WL_DBG_INFO	(1 << 1)
 #define WL_DBG_ERR	(1 << 0)
@@ -74,6 +75,13 @@ do {										\
 			printk args;						\
 		}								\
 } while (0)
+#define	WL_SCAN(args)								\
+do {									\
+	if (wl_dbg_level & WL_DBG_SCAN) {			\
+		printk(KERN_ERR "CFG80211-SCAN) %s :", __func__);	\
+		printk args;							\
+	}									\
+} while (0)
 #if (WL_DBG_LEVEL > 0)
 #define	WL_DBG(args)								\
 do {									\
@@ -87,7 +95,6 @@ do {									\
 #endif				/* (WL_DBG_LEVEL > 0) */
 
 #define WL_SCAN_RETRY_MAX	3	/* used for ibss scan */
-#define WL_NUM_SCAN_MAX		1
 #define WL_NUM_PMKIDS_MAX	MAXPMKID	/* will be used
 						 * for 2.6.33 kernel
 						 * or later
@@ -122,6 +129,7 @@ enum wl_status {
 	WL_STATUS_SCAN_ABORTING,
 	WL_STATUS_CONNECTING,
 	WL_STATUS_CONNECTED,
+	WL_STATUS_DISCONNECTING,
 	WL_STATUS_AP_CREATING,
 	WL_STATUS_AP_CREATED
 };
@@ -188,14 +196,6 @@ struct wl_conf {
 typedef s32(*EVENT_HANDLER) (struct wl_priv *wl,
                             struct net_device *ndev, const wl_event_msg_t *e, void *data);
 
-/* representing interface of cfg80211 plane */
-struct wl_iface {
-	struct wl_priv *wl;
-};
-
-struct wl_dev {
-	void *driver_data;	/* to store cfg80211 object information */
-};
 
 /* bss inform structure for cfg80211 interface */
 struct wl_cfg80211_bss_info {
@@ -274,10 +274,11 @@ struct wl_iscan_ctrl {
 };
 
 /* association inform */
+#define MAX_REQ_LINE 1024
 struct wl_connect_info {
-	u8 *req_ie;
+	u8 req_ie[MAX_REQ_LINE];
 	s32 req_ie_len;
-	u8 *resp_ie;
+	u8 resp_ie[MAX_REQ_LINE];
 	s32 resp_ie_len;
 };
 
@@ -347,7 +348,6 @@ struct wl_priv {
 	struct ether_addr bssid;	/* bssid of currently engaged network */
 
 	/* for synchronization of main event thread */
-	struct semaphore event_sync;
 	struct wl_profile *profile;	/* holding dongle profile */
 	struct wl_iscan_ctrl *iscan;	/* iscan controller */
 
@@ -357,7 +357,7 @@ struct wl_priv {
 	/* control firwmare and nvram paramter downloading */
 	struct wl_fw_ctrl *fw;
 	struct wl_pmk_list *pmk_list;	/* wpa2 pmk list */
-	struct task_struct *event_tsk;	/* task of main event handler thread */
+	tsk_ctl_t event_tsk;		/* task of main event handler thread */
 	unsigned long status;		/* current dongle status */
 	void *pub;
 	u32 channel;		/* current channel */
@@ -388,18 +388,11 @@ struct wl_priv {
 	struct p2p_info *p2p;
 	bool p2p_supported;
 	s8 last_eventmask[WL_EVENTING_MASK_LEN];
-	u8 ci[0] __attribute__ ((__aligned__(NETDEV_ALIGN)));
 };
 
-#define wl_to_dev(w) (wiphy_dev(wl->wdev->wiphy))
 #define wl_to_wiphy(w) (w->wdev->wiphy)
-#define wiphy_to_wl(w) ((struct wl_priv *)(wiphy_priv(w)))
-#define wl_to_wdev(w) (w->wdev)
-#define wdev_to_wl(w) ((struct wl_priv *)(wdev_priv(w)))
 #define wl_to_prmry_ndev(w) (w->wdev->netdev)
 #define ndev_to_wl(n) (wdev_to_wl(n->ieee80211_ptr))
-#define ci_to_wl(c) (ci->wl)
-#define wl_to_ci(w) (&w->ci)
 #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))
@@ -499,7 +492,7 @@ extern void wl_cfg80211_set_sdio_func(void *func);	/* set sdio function info */
 extern struct sdio_func *wl_cfg80211_get_sdio_func(void);	/* set sdio function info */
 extern s32 wl_cfg80211_up(void);	/* dongle up */
 extern s32 wl_cfg80211_down(void);	/* dongle down */
-extern s32 wl_cfg80211_notify_ifadd(struct net_device *net, s32 idx,
+extern s32 wl_cfg80211_notify_ifadd(struct net_device *net, s32 idx, s32 bssidx,
 int (*_net_attach)(dhd_pub_t *dhdp, int ifidx));
 extern s32 wl_cfg80211_ifdel_ops(struct net_device *net);
 extern s32 wl_cfg80211_notify_ifdel(struct net_device *net);
diff --git a/drivers/net/wireless/bcmdhd/wl_cfgp2p.c b/drivers/net/wireless/bcmdhd/wl_cfgp2p.c
index 7a5619e8ec4..98271c28658 100644
--- a/drivers/net/wireless/bcmdhd/wl_cfgp2p.c
+++ b/drivers/net/wireless/bcmdhd/wl_cfgp2p.c
@@ -176,8 +176,8 @@ wl_cfgp2p_ifdel(struct wl_priv *wl, struct ether_addr *mac)
 	s32 ret;
 	struct net_device *netdev = wl_to_prmry_ndev(wl);
 
-	CFGP2P_INFO(("---wl p2p_ifdel %02x:%02x:%02x:%02x:%02x:%02x\n",
-	    mac->octet[0], mac->octet[1], mac->octet[2],
+	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),
@@ -200,7 +200,7 @@ wl_cfgp2p_ifchange(struct wl_priv *wl, struct ether_addr *mac, u8 if_type,
 {
 	wl_p2p_if_t ifreq;
 	s32 err;
-	struct net_device *netdev = wl_to_prmry_ndev(wl);
+	struct net_device *netdev =  wl_to_p2p_bss_ndev(wl, P2PAPI_BSSCFG_CONNECTION);
 
 	ifreq.type = if_type;
 	ifreq.chspec = chspec;
@@ -970,8 +970,7 @@ wl_cfgp2p_discover_listen(struct wl_priv *wl, s32 channel, u32 duration_ms)
 	} while (0);
 
 	s32 ret = BCME_OK;
-	CFGP2P_DBG((" Enter\n"));
-	CFGP2P_INFO(("Channel : %d, Duration : %d\n", channel, duration_ms));
+	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"));
@@ -996,9 +995,9 @@ wl_cfgp2p_discover_listen(struct wl_priv *wl, s32 channel, u32 duration_ms)
 	}
 
 	/*  We will wait to receive WLC_E_P2P_DISC_LISTEN_COMPLETE from dongle , 
-	 *  otherwise we will wait up to duration_ms + 10ms
+	 *  otherwise we will wait up to duration_ms + 200ms
 	 */
-	INIT_TIMER(wl->p2p->listen_timer, wl_cfgp2p_listen_expired, duration_ms, 20);
+	INIT_TIMER(wl->p2p->listen_timer, wl_cfgp2p_listen_expired, duration_ms, 200);
 
 #undef INIT_TIMER
 exit:
@@ -1048,10 +1047,13 @@ wl_cfgp2p_action_tx_complete(struct wl_priv *wl, struct net_device *ndev,
 	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)
+		if (status == WLC_E_STATUS_SUCCESS) {
 			wl_set_p2p_status(wl, ACTION_TX_COMPLETED);
-		else
+		}
+		else {
+			wl_set_p2p_status(wl, ACTION_TX_NOACK);
 			CFGP2P_ERR(("WLC_E_ACTION_FRAME_COMPLETE : NO ACK\n"));
+		}
 		wake_up_interruptible(&wl->dongle_event_wait);
 	} else {
 		CFGP2P_INFO((" WLC_E_ACTION_FRAME_OFFCHAN_COMPLETE is received,"
@@ -1082,6 +1084,7 @@ wl_cfgp2p_tx_action_frame(struct wl_priv *wl, struct net_device *dev,
 	    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);
@@ -1095,7 +1098,7 @@ wl_cfgp2p_tx_action_frame(struct wl_priv *wl, struct net_device *dev,
 		goto exit;
 	}
 	timeout = wait_event_interruptible_timeout(wl->dongle_event_wait,
-	                (wl_get_p2p_status(wl, ACTION_TX_COMPLETED) == TRUE),
+	(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)) {
diff --git a/drivers/net/wireless/bcmdhd/wl_cfgp2p.h b/drivers/net/wireless/bcmdhd/wl_cfgp2p.h
index d62f5422708..b08504d8f95 100644
--- a/drivers/net/wireless/bcmdhd/wl_cfgp2p.h
+++ b/drivers/net/wireless/bcmdhd/wl_cfgp2p.h
@@ -88,6 +88,7 @@ enum wl_cfgp2p_status {
 	WLP2P_STATUS_IF_CHANGED,
 	WLP2P_STATUS_LISTEN_EXPIRED,
 	WLP2P_STATUS_ACTION_TX_COMPLETED,
+	WLP2P_STATUS_ACTION_TX_NOACK,
 	WLP2P_STATUS_SCANNING
 };
 
diff --git a/drivers/net/wireless/bcmdhd/wl_iw.c b/drivers/net/wireless/bcmdhd/wl_iw.c
index ae28c6d0215..6d546fcd396 100644
--- a/drivers/net/wireless/bcmdhd/wl_iw.c
+++ b/drivers/net/wireless/bcmdhd/wl_iw.c
@@ -792,7 +792,7 @@ wl_iw_set_power_mode(
 
 #ifdef COEX_DHCP
 		g_bt->ts_dhcp_start = JF2MS;
-		g_bt->dhcp_done = false;
+		g_bt->dhcp_done = FALSE;
 		WL_TRACE_COEX(("%s: DHCP start, pm:%d changed to pm:%d\n",
 			__FUNCTION__, pm, pm_local));
 
@@ -806,7 +806,7 @@ wl_iw_set_power_mode(
 		net_os_set_packet_filter(dev, 1);
 
 #ifdef COEX_DHCP
-		g_bt->dhcp_done = true;
+		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));
@@ -828,7 +828,7 @@ wl_iw_set_power_mode(
 bool btcoex_is_sco_active(struct net_device *dev)
 {
 	int ioc_res = 0;
-	bool res = false;
+	bool res = FALSE;
 	int sco_id_cnt = 0;
 	int param27;
 	int i;
@@ -852,7 +852,7 @@ bool btcoex_is_sco_active(struct net_device *dev)
 		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;
+			res = TRUE;
 			break;
 		}
 
@@ -866,7 +866,7 @@ bool btcoex_is_sco_active(struct net_device *dev)
 
 static int set_btc_esco_params(struct net_device *dev, bool trump_sco)
 {
-	static bool saved_status = false;
+	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 };
@@ -902,7 +902,7 @@ static int set_btc_esco_params(struct net_device *dev, bool trump_sco)
 		} else {
 			WL_ERROR((":%s: save btc_params failed\n",
 				__FUNCTION__));
-			saved_status = false;
+			saved_status = FALSE;
 			return -1;
 		}
 
@@ -920,7 +920,7 @@ static int set_btc_esco_params(struct net_device *dev, bool trump_sco)
 		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;
+		saved_status = TRUE;
 
 	} else if (saved_status) {
 		
@@ -946,7 +946,7 @@ static int set_btc_esco_params(struct net_device *dev, bool trump_sco)
 			saved_reg50, saved_reg51, saved_reg64,
 			saved_reg65, saved_reg71));
 
-		saved_status = false;
+		saved_status = FALSE;
 	} else {
 		WL_ERROR((":%s att to restore not saved BTCOEX params\n",
 			__FUNCTION__));
@@ -6320,12 +6320,12 @@ fail:
 #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;
@@ -6354,42 +6354,57 @@ get_softap_auto_channel(struct net_device *dev, struct ap_profile *ap)
 		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;
-			}
+
+	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);
+		bcm_mdelay(350);
 
-			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 {
+	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 = %d\n", ret, chosen));
+					          "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 == 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;
-			}
+		}
+
+		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)
+	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;
 } 
 
@@ -6482,6 +6497,15 @@ set_ap_cfg(struct net_device *dev, struct ap_profile *ap)
 			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;
@@ -6528,16 +6552,16 @@ set_ap_cfg(struct net_device *dev, struct ap_profile *ap)
 	
 	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",
+		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__));
-		goto fail;
 	}
 
+
 	if (ap_cfg_running == FALSE) {
 		updown = 0;
 		if ((res = dev_wlc_ioctl(dev, WLC_UP, &updown, sizeof(updown)))) {
@@ -6877,11 +6901,11 @@ static int wl_iw_softap_deassoc_stations(struct net_device *dev, u8 *mac)
 	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;
+	bool deauth_all = FALSE;
 
 	
 	if (mac == NULL) {
-		deauth_all = true;
+		deauth_all = TRUE;
 		sta_mac = z_mac;  
 	} else {
 		sta_mac = mac;  
@@ -7179,7 +7203,7 @@ set_ap_mac_list(struct net_device *dev, void *buf)
 		if (assoc_maclist->count)
 			for (i = 0; i < assoc_maclist->count; i++) {
 				int j;
-				bool assoc_mac_matched = false;
+				bool assoc_mac_matched = FALSE;
 				
 				WL_SOFTAP(("\n Cheking assoc STA: "));
 				dhd_print_buf(&assoc_maclist->ea[i], 6, 7);
@@ -7189,7 +7213,7 @@ set_ap_mac_list(struct net_device *dev, void *buf)
 					if (!bcmp(&assoc_maclist->ea[i], &maclist->ea[j],
 						ETHER_ADDR_LEN)) {
 						
-						assoc_mac_matched = true;
+						assoc_mac_matched = TRUE;
 						break;
 					}
 
diff --git a/drivers/power/ab5500_charger.c b/drivers/power/ab5500_charger.c
index f57e2088a4a..b90c51a4f31 100644
--- a/drivers/power/ab5500_charger.c
+++ b/drivers/power/ab5500_charger.c
@@ -1309,7 +1309,8 @@ static irqreturn_t ab5500_charger_usblinkstatus_handler(int irq, void *_di)
 
 	dev_dbg(di->dev, "USB link status changed\n");
 
-	queue_work(di->charger_wq, &di->usb_link_status_work);
+	if (!di->usb.charger_online)
+		queue_work(di->charger_wq, &di->usb_link_status_work);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/power/ab8500_charger.c b/drivers/power/ab8500_charger.c
index f0d04c7c6d3..ffb66b8e627 100644
--- a/drivers/power/ab8500_charger.c
+++ b/drivers/power/ab8500_charger.c
@@ -2331,6 +2331,9 @@ static int ab8500_charger_usb_notifier_call(struct notifier_block *nb,
 	else /* Should never occur */
 		bm_usb_state = AB8500_BM_USB_STATE_RESET_FS;
 
+	if (di == NULL)
+		return;
+
 	dev_dbg(di->dev, "%s usb_state: 0x%02x mA: %d\n",
 		__func__, bm_usb_state, mA);
 
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index 531bdbeede8..fa76ce7678a 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -555,10 +555,10 @@ static struct logger_log VAR = { \
 	.size = SIZE, \
 };
 
-DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 64*1024)
+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, 64*1024)
-DEFINE_LOGGER_DEVICE(log_system, LOGGER_LOG_SYSTEM, 64*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)
 {
diff --git a/drivers/staging/cg2900/board-ux500-cg2900.c b/drivers/staging/cg2900/board-ux500-cg2900.c
index 1f4b93ca0e5..910a3c0499a 100644
--- a/drivers/staging/cg2900/board-ux500-cg2900.c
+++ b/drivers/staging/cg2900/board-ux500-cg2900.c
@@ -32,9 +32,10 @@
 #define CG2900_BT_ENABLE_GPIO		170
 #define CG2900_GBF_ENA_RESET_GPIO	171
 #define WLAN_PMU_EN_GPIO		226
+#define WLAN_PMU_EN_GPIO_SNOWBALL	161
 #define WLAN_PMU_EN_GPIO_U9500		AB8500_PIN_GPIO11
-#define CG2900_UX500_BT_CTS_GPIO		0
-#define CG2900_U5500_BT_CTS_GPIO		168
+#define CG2900_UX500_BT_CTS_GPIO	0
+#define CG2900_U5500_BT_CTS_GPIO	168
 
 enum cg2900_gpio_pull_sleep ux500_cg2900_sleep_gpio[21] = {
 	CG2900_NO_PULL,		/* GPIO 0:  PTA_CONFX */
@@ -160,6 +161,34 @@ static struct resource cg2900_uart_resources_u8500[] = {
 	},
 };
 
+static struct resource cg2900_uart_resources_snowball[] = {
+	{
+		.start = CG2900_GBF_ENA_RESET_GPIO,
+		.end = CG2900_GBF_ENA_RESET_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "gbf_ena_reset",
+	},
+	{
+		.start = WLAN_PMU_EN_GPIO_SNOWBALL,
+		.end = WLAN_PMU_EN_GPIO_SNOWBALL,
+		.flags = IORESOURCE_IO,
+		.name = "pmu_en",
+	},
+	{
+		.start = CG2900_UX500_BT_CTS_GPIO,
+		.end = CG2900_UX500_BT_CTS_GPIO,
+		.flags = IORESOURCE_IO,
+		.name = "cts_gpio",
+	},
+	{
+		.start = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.end = NOMADIK_GPIO_TO_IRQ(CG2900_UX500_BT_CTS_GPIO),
+		.flags = IORESOURCE_IRQ,
+		.name = "cts_irq",
+	},
+};
+
+
 static struct resource cg2900_uart_resources_u9500[] = {
 	{
 		.start = CG2900_GBF_ENA_RESET_GPIO,
@@ -275,15 +304,21 @@ static int __init board_cg2900_init(void)
 		if (machine_is_hrefv60()) {
 			/* u8500 */
 			ux500_cg2900_uart_device.num_resources =
-					ARRAY_SIZE(cg2900_uart_resources_u8500);
+				ARRAY_SIZE(cg2900_uart_resources_u8500);
+			ux500_cg2900_uart_device.resource =
+				cg2900_uart_resources_u8500;
+		} else if (machine_is_snowball()) {
+			/* snowball have diffrent PMU_EN gpio */
+			ux500_cg2900_uart_device.num_resources =
+				ARRAY_SIZE(cg2900_uart_resources_snowball);
 			ux500_cg2900_uart_device.resource =
-					cg2900_uart_resources_u8500;
+				cg2900_uart_resources_snowball;
 		} else {
 			/* u8500 pre v60*/
 			ux500_cg2900_uart_device.num_resources =
-					ARRAY_SIZE(cg2900_uart_resources_pre_v60);
+				ARRAY_SIZE(cg2900_uart_resources_pre_v60);
 			ux500_cg2900_uart_device.resource =
-					cg2900_uart_resources_pre_v60;
+				cg2900_uart_resources_pre_v60;
 		}
 	} else if (cpu_is_u5500()) {
 		/* u5500 */
diff --git a/drivers/staging/mmio/st_mmio.c b/drivers/staging/mmio/st_mmio.c
index da30844b28c..febbb1b3487 100644
--- a/drivers/staging/mmio/st_mmio.c
+++ b/drivers/staging/mmio/st_mmio.c
@@ -245,7 +245,7 @@ static int copy_user_buffer(void __iomem **dest_buf,
 
 	*dest_buf = kmalloc(size, GFP_KERNEL);
 
-	if (!dest_buf) {
+	if (!*dest_buf) {
 		err = -ENOMEM;
 		goto nomem;
 	}
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index e0cfb4acc0b..fb8809d7d04 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -2252,6 +2252,13 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 		kfree(uap);
 	}
  out:
+	/*
+	 * Disable the silicon block pclk and any voltage domain and just
+	 * power it up and clock it when it's needed
+	 */
+	amba_pclk_disable(dev);
+	amba_vcore_disable(dev);
+
 	return ret;
 }
 
diff --git a/drivers/usb/gadget/android.c b/drivers/usb/gadget/android.c
index 64399e42edb..805914f60a5 100644
--- a/drivers/usb/gadget/android.c
+++ b/drivers/usb/gadget/android.c
@@ -524,7 +524,7 @@ static int rndis_function_bind_config(struct android_usb_function *f,
 		rndis->ethaddr[0], rndis->ethaddr[1], rndis->ethaddr[2],
 		rndis->ethaddr[3], rndis->ethaddr[4], rndis->ethaddr[5]);
 
-	ret = gether_setup(c->cdev->gadget, rndis->ethaddr);
+	ret = gether_setup_name(c->cdev->gadget, rndis->ethaddr, "rndis");
 	if (ret) {
 		pr_err("%s: gether_setup failed\n", __func__);
 		return ret;
@@ -1264,7 +1264,6 @@ 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	**functions = dev->functions;
 	struct android_usb_function	*f;
 	int value = -EOPNOTSUPP;
 	unsigned long flags;
@@ -1274,7 +1273,7 @@ android_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *c)
 	req->length = 0;
 	gadget->ep0->driver_data = cdev;
 
-	while ((f = *functions++)) {
+	list_for_each_entry(f, &dev->enabled_functions, enabled_list) {
 		if (f->ctrlrequest) {
 			value = f->ctrlrequest(f, cdev, c);
 			if (value >= 0)
diff --git a/drivers/usb/gadget/epautoconf.c b/drivers/usb/gadget/epautoconf.c
index 9b7360ff5aa..1951a11f947 100644
--- a/drivers/usb/gadget/epautoconf.c
+++ b/drivers/usb/gadget/epautoconf.c
@@ -292,6 +292,12 @@ struct usb_ep *usb_ep_autoconfig (
 #endif
 	}
 
+	if (gadget->ops->configure_ep) {
+		ep = gadget->ops->configure_ep(gadget, type, desc);
+		if (ep && ep_matches(gadget, ep, desc))
+			return ep;
+	}
+
 	/* Second, look at endpoints until an unclaimed one looks usable */
 	list_for_each_entry (ep, &gadget->ep_list, ep_list) {
 		if (ep_matches (gadget, ep, desc))
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index 6590501c29e..b5a30fee014 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -765,6 +765,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;
@@ -787,7 +807,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,
diff --git a/drivers/usb/gadget/u_ether.h b/drivers/usb/gadget/u_ether.h
index 5bb1021f6ab..64b65f92168 100644
--- a/drivers/usb/gadget/u_ether.h
+++ b/drivers/usb/gadget/u_ether.h
@@ -86,6 +86,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 *);
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index 67ee65ad637..988bc908fc0 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -1041,9 +1041,6 @@ static void musb_shutdown(struct platform_device *pdev)
 #if defined(CONFIG_USB_MUSB_TUSB6010) || defined(CONFIG_USB_MUSB_OMAP2PLUS) \
 	|| defined(CONFIG_USB_MUSB_AM35X)
 static ushort __initdata fifo_mode = 4;
-#elif defined(CONFIG_USB_MUSB_UX500) \
-	|| defined(CONFIG_USB_MUSB_UX500_MODULE)
-static ushort __initdata fifo_mode = 5;
 #else
 static ushort __initdata fifo_mode = 2;
 #endif
@@ -2407,7 +2404,7 @@ static const struct dev_pm_ops musb_dev_pm_ops = {
 	.runtime_resume = musb_runtime_resume,
 };
 
-#define MUSB_DEV_PM_OPS (&musb_dev_pm_ops)
+#define MUSB_DEV_PM_OPS NULL
 #else
 #define	MUSB_DEV_PM_OPS	NULL
 #endif
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 0e053b58796..c4cd091a9f5 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -279,6 +279,8 @@ struct musb_platform_ops {
 	int	(*adjust_channel_params)(struct dma_channel *channel,
 				u16 packet_sz, u8 *mode,
 				dma_addr_t *dma_addr, u32 *len);
+	struct usb_ep* (*configure_endpoints)(struct musb *musb, u8 type,
+				struct usb_endpoint_descriptor  *desc);
 };
 
 /*
@@ -665,4 +667,13 @@ static inline int musb_platform_exit(struct musb *musb)
 	return musb->ops->exit(musb);
 }
 
+static inline struct usb_ep *musb_platform_configure_ep(struct musb *musb,
+		u8 type, struct usb_endpoint_descriptor  *desc)
+{
+	struct usb_ep *ep = NULL;
+
+	if (musb->ops->configure_endpoints)
+		ep = musb->ops->configure_endpoints(musb, type, desc);
+	return ep;
+}
 #endif	/* __MUSB_CORE_H__ */
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index f3fd718554d..ab017cbb78e 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -1733,6 +1733,14 @@ static int musb_gadget_pullup(struct usb_gadget *gadget, int is_on)
 	return 0;
 }
 
+static struct usb_ep *musb_gadget_configure_ep(struct usb_gadget *gadget,
+		u8 type, struct usb_endpoint_descriptor *desc)
+{
+	struct musb	*musb = gadget_to_musb(gadget);
+
+	return musb_platform_configure_ep(musb, type, desc);
+}
+
 static const struct usb_gadget_ops musb_gadget_operations = {
 	.get_frame		= musb_gadget_get_frame,
 	.wakeup			= musb_gadget_wakeup,
@@ -1740,6 +1748,7 @@ static const struct usb_gadget_ops musb_gadget_operations = {
 	/* .vbus_session		= musb_gadget_vbus_session, */
 	.vbus_draw		= musb_gadget_vbus_draw,
 	.pullup			= musb_gadget_pullup,
+	.configure_ep		= musb_gadget_configure_ep,
 };
 
 /* ----------------------------------------------------------------------- */
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index 07e25fa1bf2..f12b046b0d6 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -1169,7 +1169,6 @@ void musb_host_tx(struct musb *musb, u8 epnum)
 		} while ((tx_csr & MUSB_TXCSR_TXPKTRDY) != 0);
 		dev_dbg(musb->controller, "TXPKTRDY Cleared. Continue...\n");
 
-		return;
 	} else if (tx_csr & MUSB_TXCSR_H_NAKTIMEOUT) {
 		dev_dbg(musb->controller, "TX end=%d device not responding\n", epnum);
 
diff --git a/drivers/usb/musb/ux500.c b/drivers/usb/musb/ux500.c
index bae06ec7d67..ce169460148 100644
--- a/drivers/usb/musb/ux500.c
+++ b/drivers/usb/musb/ux500.c
@@ -59,6 +59,7 @@ void ux500_store_context(struct musb *musb)
 	if (is_host_enabled(musb)) {
 		context.frame = musb_readw(musb_base, MUSB_FRAME);
 		context.testmode = musb_readb(musb_base, MUSB_TESTMODE);
+		context.busctl = musb_read_ulpi_buscontrol(musb->mregs);
 	}
 	context.power = musb_readb(musb_base, MUSB_POWER);
 	context.intrtxe = musb_readw(musb_base, MUSB_INTRTXE);
@@ -66,8 +67,19 @@ void ux500_store_context(struct musb *musb)
 	context.intrusbe = musb_readb(musb_base, MUSB_INTRUSBE);
 	context.index = musb_readb(musb_base, MUSB_INDEX);
 	context.devctl = DEFAULT_DEVCTL;
+
 	for (i = 0; i < musb->config->num_eps; ++i) {
-		epio = musb->endpoints[i].regs;
+		struct musb_hw_ep       *hw_ep;
+
+		musb_writeb(musb_base, MUSB_INDEX, i);
+		hw_ep = &musb->endpoints[i];
+		if (!hw_ep)
+			continue;
+
+		epio = hw_ep->regs;
+		if (!epio)
+			continue;
+
 		context.index_regs[i].txmaxp =
 				musb_readw(epio, MUSB_TXMAXP);
 		context.index_regs[i].txcsr =
@@ -132,6 +144,7 @@ void ux500_restore_context(void)
 	if (is_host_enabled(musb)) {
 		musb_writew(musb_base, MUSB_FRAME, context.frame);
 		musb_writeb(musb_base, MUSB_TESTMODE, context.testmode);
+		musb_write_ulpi_buscontrol(musb->mregs, context.busctl);
 	 }
 	musb_writeb(musb_base, MUSB_POWER, context.power);
 	musb_writew(musb_base, MUSB_INTRTXE, context.intrtxe);
@@ -140,7 +153,17 @@ void ux500_restore_context(void)
 	musb_writeb(musb_base, MUSB_DEVCTL, context.devctl);
 
 	for (i = 0; i < musb->config->num_eps; ++i) {
-		epio = musb->endpoints[i].regs;
+		struct musb_hw_ep       *hw_ep;
+
+		musb_writeb(musb_base, MUSB_INDEX, i);
+		hw_ep = &musb->endpoints[i];
+		if (!hw_ep)
+			continue;
+
+		epio = hw_ep->regs;
+		if (!epio)
+			continue;
+
 		musb_writew(epio, MUSB_TXMAXP,
 			context.index_regs[i].txmaxp);
 		musb_writew(epio, MUSB_TXCSR,
@@ -205,11 +228,15 @@ static void musb_notify_idle(unsigned long _musb)
 
 	switch (musb->xceiv->state) {
 	case OTG_STATE_A_WAIT_BCON:
-		devctl &= ~MUSB_DEVCTL_SESSION;
-		musb_writeb(musb->mregs, MUSB_DEVCTL, devctl);
-		musb->xceiv->state = OTG_STATE_B_IDLE;
-		MUSB_DEV_MODE(musb);
+		if (devctl & MUSB_DEVCTL_BDEVICE) {
+			musb->xceiv->state = OTG_STATE_B_IDLE;
+			MUSB_DEV_MODE(musb);
+		} else {
+			musb->xceiv->state = OTG_STATE_A_IDLE;
+			MUSB_HST_MODE(musb);
+		}
 		break;
+
 	case OTG_STATE_A_SUSPEND:
 	default:
 		break;
@@ -239,6 +266,8 @@ static int musb_otg_notifications(struct notifier_block *nb,
 
 	case USB_EVENT_NONE:
 		dev_dbg(musb->controller, "VBUS Disconnect\n");
+		if (is_otg_enabled(musb))
+			ux500_musb_set_vbus(musb, 0);
 
 		break;
 	default:
@@ -340,10 +369,34 @@ static void ux500_musb_try_idle(struct musb *musb, unsigned long timeout)
 		(unsigned long)jiffies_to_msecs(timeout - jiffies));
 	mod_timer(&notify_timer, timeout);
 }
+
 static void ux500_musb_enable(struct musb *musb)
 {
 	ux500_store_context(musb);
 }
+
+static struct usb_ep *ux500_musb_configure_endpoints(struct musb *musb,
+		u8 type, struct usb_endpoint_descriptor  *desc)
+{
+	struct usb_ep *ep = NULL;
+	struct usb_gadget *gadget = &musb->g;
+	char name[4];
+
+	if (USB_ENDPOINT_XFER_INT == type) {
+		list_for_each_entry(ep, &gadget->ep_list, ep_list) {
+			if (ep->maxpacket == 512)
+				continue;
+			if (NULL == ep->driver_data) {
+				strncpy(name, (ep->name + 3), 4);
+				if (USB_DIR_IN & desc->bEndpointAddress)
+					if (strcmp("in", name) == 0)
+						return ep;
+			}
+		}
+	}
+	return ep;
+}
+
 static int ux500_musb_init(struct musb *musb)
 {
 	int status;
@@ -389,6 +442,7 @@ static const struct musb_platform_ops ux500_ops = {
 	.try_idle	= ux500_musb_try_idle,
 
 	.enable		= ux500_musb_enable,
+	.configure_endpoints	= ux500_musb_configure_endpoints,
 };
 
 /**
diff --git a/drivers/usb/musb/ux500_dma.c b/drivers/usb/musb/ux500_dma.c
index d6a606ef839..fc0d3a04859 100644
--- a/drivers/usb/musb/ux500_dma.c
+++ b/drivers/usb/musb/ux500_dma.c
@@ -32,6 +32,11 @@
 #include <linux/pfn.h>
 #include <mach/usb.h>
 #include "musb_core.h"
+#undef DBG
+#undef WARNING
+#undef INFO
+#include <linux/usb/composite.h>
+#define Ux500_USB_DMA_MIN_TRANSFER_SIZE	512
 
 struct ux500_dma_channel {
 	struct dma_channel channel;
@@ -258,13 +263,44 @@ static void ux500_dma_channel_release(struct dma_channel *channel)
 static int ux500_dma_is_compatible(struct dma_channel *channel,
 		u16 maxpacket, void *buf, u32 length)
 {
-	if ((maxpacket & 0x3)		||
-		((int)buf & 0x3)	||
-		(length < 512)		||
-		(length & 0x3))
-		return false;
-	else
-		return true;
+	struct ux500_dma_channel *ux500_channel = channel->private_data;
+	struct musb_hw_ep       *hw_ep = ux500_channel->hw_ep;
+	struct musb *musb = hw_ep->musb;
+	struct usb_descriptor_header **descriptors;
+	struct usb_function		*f;
+	struct usb_gadget		*gadget = &musb->g;
+	struct usb_composite_dev	*cdev = get_gadget_data(gadget);
+
+	if (length < Ux500_USB_DMA_MIN_TRANSFER_SIZE)
+		return 0;
+
+	list_for_each_entry(f, &cdev->config->functions, list) {
+		if (!strcmp(f->name, "cdc_ethernet") ||
+			!strcmp(f->name, "rndis") ||
+			!strcmp(f->name, "phonet") ||
+			!strcmp(f->name, "adb")) {
+			if (gadget->speed == USB_SPEED_HIGH)
+				descriptors = f->hs_descriptors;
+			else
+				descriptors = f->descriptors;
+
+			for (; *descriptors; ++descriptors) {
+				struct usb_endpoint_descriptor *ep;
+
+				if ((*descriptors)->bDescriptorType !=
+					USB_DT_ENDPOINT)
+					continue;
+
+				ep = (struct usb_endpoint_descriptor *)
+					*descriptors;
+				if (ep->bEndpointAddress ==
+					ux500_channel->hw_ep->epnum)
+					return 0;
+			}
+		}
+	}
+
+	return 1;
 }
 
 /**
@@ -282,12 +318,15 @@ static int ux500_dma_channel_program(struct dma_channel *channel,
 				dma_addr_t dma_addr, u32 len)
 {
 	int ret;
+	struct ux500_dma_channel *ux500_dma_channel = channel->private_data;
 
 	BUG_ON(channel->status == MUSB_DMA_STATUS_UNKNOWN ||
 		channel->status == MUSB_DMA_STATUS_BUSY);
 
-	if (!ux500_dma_is_compatible(channel, packet_sz, (void *)dma_addr, len))
-		return false;
+	if (len < Ux500_USB_DMA_MIN_TRANSFER_SIZE)
+		return 0;
+	if (!ux500_dma_channel->is_tx && len < packet_sz)
+		return 0;
 
 	channel->status = MUSB_DMA_STATUS_BUSY;
 	channel->actual_len = 0;
diff --git a/drivers/usb/otg/ab5500-usb.c b/drivers/usb/otg/ab5500-usb.c
index 7290c2d199c..3b4d6bda1cb 100644
--- a/drivers/usb/otg/ab5500-usb.c
+++ b/drivers/usb/otg/ab5500-usb.c
@@ -258,6 +258,10 @@ static int ab5500_usb_link_status_update(struct ab5500_usb *ab)
 		event = USB_EVENT_ID;
 
 		break;
+	case USB_LINK_DEDICATED_CHG:
+		/* TODO: vbus_draw */
+		event = USB_EVENT_CHARGER;
+		break;
 	default:
 		break;
 	}
diff --git a/drivers/usb/otg/ab8500-usb.c b/drivers/usb/otg/ab8500-usb.c
index 19446ba114d..cd4b81b259f 100644
--- a/drivers/usb/otg/ab8500-usb.c
+++ b/drivers/usb/otg/ab8500-usb.c
@@ -37,7 +37,7 @@
 #include <linux/mfd/dbx500-prcmu.h>
 #include <mach/usb.h>
 #include <linux/kernel_stat.h>
-
+#include <linux/pm_qos_params.h>
 #include <linux/wakelock.h>
 static struct wake_lock ab8500_musb_wakelock;
 
@@ -71,6 +71,8 @@ static struct wake_lock ab8500_musb_wakelock;
 #define AB8500_USB_PHY_TUNE2	0x06
 #define AB8500_USB_PHY_TUNE3	0x07
 
+static struct pm_qos_request_list usb_pm_qos_latency;
+static bool usb_pm_qos_is_latency_0;
 
 #define USB_PROBE_DELAY 1000 /* 1 seconds */
 #define USB_LIMIT (200) /* If we have more than 200 irqs per second */
@@ -170,13 +172,27 @@ static void ab8500_usb_load(struct work_struct *work)
 	num_irqs += kstat_irqs_cpu(IRQ_DB8500_USBOTG, cpu);
 
 	if ((num_irqs > old_num_irqs) &&
-		(num_irqs - old_num_irqs) > USB_LIMIT)
-			prcmu_qos_update_requirement(PRCMU_QOS_ARM_OPP,
+		(num_irqs - old_num_irqs) > USB_LIMIT) {
+
+		prcmu_qos_update_requirement(PRCMU_QOS_ARM_OPP,
 							"usb", 125);
-	else
-			prcmu_qos_update_requirement(PRCMU_QOS_ARM_OPP,
-							"usb", 25);
+		if (!usb_pm_qos_is_latency_0) {
+
+			pm_qos_add_request(&usb_pm_qos_latency,
+						PM_QOS_CPU_DMA_LATENCY, 0);
+			usb_pm_qos_is_latency_0 = true;
+		}
+	} else {
+
+		if (usb_pm_qos_is_latency_0) {
+
+				pm_qos_remove_request(&usb_pm_qos_latency);
+				usb_pm_qos_is_latency_0 = false;
+		}
 
+		prcmu_qos_update_requirement(PRCMU_QOS_ARM_OPP,
+							"usb", 25);
+	}
 	old_num_irqs = num_irqs;
 
 	schedule_delayed_work_on(0,
@@ -221,6 +237,7 @@ static void ab8500_usb_regulator_ctrl(struct ab8500_usb *ab, bool sel_host,
 	}
 }
 
+
 static void ab8500_usb_phy_enable(struct ab8500_usb *ab, bool sel_host)
 {
 	u8 bit;
@@ -235,11 +252,10 @@ static void ab8500_usb_phy_enable(struct ab8500_usb *ab, bool sel_host)
 
 	prcmu_qos_update_requirement(PRCMU_QOS_APE_OPP,
 				(char *)dev_name(ab->dev), 100);
-	if (!sel_host) {
-		schedule_delayed_work_on(0,
+
+	schedule_delayed_work_on(0,
 					&ab->work_usb_workaround,
 					msecs_to_jiffies(USB_PROBE_DELAY));
-	}
 
 	abx500_mask_and_set_register_interruptible(ab->dev,
 				AB8500_USB,
@@ -399,10 +415,17 @@ static void ab8500_usb_delayed_work(struct work_struct *work)
 static irqreturn_t ab8500_usb_disconnect_irq(int irq, void *data)
 {
 	struct ab8500_usb *ab = (struct ab8500_usb *) data;
+	enum usb_xceiv_events event;
 
 	/* Link status will not be updated till phy is disabled. */
-	if (ab->mode == USB_HOST)
+	if (ab->mode == USB_HOST) {
+		event = USB_EVENT_NONE;
+		ab->otg.default_a = false;
+		ab->vbus_draw = 0;
+		atomic_notifier_call_chain(&ab->otg.notifier,
+					event, &ab->vbus_draw);
 		ab8500_usb_host_phy_dis(ab);
+	}
 	else if (ab->mode == USB_PERIPHERAL)
 		ab8500_usb_peri_phy_dis(ab);
 	else if (ab->mode == USB_DEDICATED_CHG && ab->rev == 0x20) {
@@ -486,23 +509,13 @@ static int ab8500_usb_set_peripheral(struct otg_transceiver *otg,
 
 	ab = xceiv_to_ab(otg);
 
+	ab->otg.gadget = gadget;
 	/* Some drivers call this function in atomic context.
 	 * Do not update ab8500 registers directly till this
 	 * is fixed.
 	 */
-
-	if (!gadget) {
-		ab->otg.gadget = NULL;
+	if (!gadget)
 		schedule_work(&ab->phy_dis_work);
-	} else {
-		ab->otg.gadget = gadget;
-
-		/* Phy will not be enabled if cable is already
-		 * plugged-in. Schedule to enable phy.
-		 * Use same delay to avoid any race condition.
-		 */
-		schedule_delayed_work(&ab->dwork, ab->link_status_wait);
-	}
 
 	return 0;
 }
@@ -517,22 +530,14 @@ static int ab8500_usb_set_host(struct otg_transceiver *otg,
 
 	ab = xceiv_to_ab(otg);
 
+	ab->otg.host = host;
+
 	/* Some drivers call this function in atomic context.
 	 * Do not update ab8500 registers directly till this
 	 * is fixed.
 	 */
-
-	if (!host) {
-		ab->otg.host = NULL;
+	if (!host)
 		schedule_work(&ab->phy_dis_work);
-	} else {
-		ab->otg.host = host;
-		/* Phy will not be enabled if cable is already
-		 * plugged-in. Schedule to enable phy.
-		 * Use same delay to avoid any race condition.
-		 */
-		schedule_delayed_work(&ab->dwork, ab->link_status_wait);
-	}
 
 	return 0;
 }
@@ -544,11 +549,6 @@ static int ab8500_usb_set_host(struct otg_transceiver *otg,
  */
 static int ab8500_usb_boot_detect(struct ab8500_usb *ab)
 {
-	int err;
-	struct device *device = ab->dev;
-	u8 usb_status = 0;
-	u8 val = 0;
-
 	/* Disabling PHY before selective enable or disable */
 	abx500_mask_and_set_register_interruptible(ab->dev,
 				AB8500_USB,
@@ -578,39 +578,7 @@ static int ab8500_usb_boot_detect(struct ab8500_usb *ab)
 				AB8500_BIT_PHY_CTRL_HOST_EN,
 				0);
 
-
-	err = abx500_get_register_interruptible(device,
-		AB8500_INTERRUPT, AB8500_IT_SOURCE20_REG,
-		&usb_status);
-	if (err < 0) {
-		dev_err(device, "Read IT 20 failed\n");
-		return err;
-	}
-
-	if (usb_status & AB8500_SRC_INT_USB_HOST)
-		ab8500_usb_host_phy_en(ab);
-
-
-	err = abx500_get_register_interruptible(device,
-		AB8500_INTERRUPT, AB8500_IT_SOURCE2_REG,
-		&usb_status);
-	if (err < 0) {
-		dev_err(device, "Read IT 2 failed\n");
-		return err;
-	}
-
-	if (usb_status & AB8500_SRC_INT_USB_DEVICE) {
-		/* Check if it is a dedicated charger */
-		(void)abx500_get_register_interruptible(device,
-		AB8500_USB, AB8500_USB_LINE_STAT_REG, &val);
-
-		val = (val >> 3) & 0x0F;
-
-	if (val == USB_LINK_DEDICATED_CHG)
-		ab->mode = USB_DEDICATED_CHG;
-	else
-		ab8500_usb_peri_phy_en(ab);
-	}
+	ab8500_usb_link_status_update(ab);
 
 	return 0;
 }
diff --git a/drivers/usb/otg/otg_id.c b/drivers/usb/otg/otg_id.c
index 2398b1a951d..ce22b462130 100644
--- a/drivers/usb/otg/otg_id.c
+++ b/drivers/usb/otg/otg_id.c
@@ -44,16 +44,24 @@ 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) {
-		ret = otg_id_nb->detect(otg_id_nb);
+		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");
diff --git a/drivers/video/av8100/av8100.c b/drivers/video/av8100/av8100.c
index 356e0061335..9e4432a3ea0 100644
--- a/drivers/video/av8100/av8100.c
+++ b/drivers/video/av8100/av8100.c
@@ -52,7 +52,7 @@
 /* Standby search time */
 #define AV8100_ON_TIME 1	/* 9 ms step */
 #define AV8100_DENC_OFF_TIME 3	/* 275 ms step if > V1. Not used if V1 */
-#define AV8100_HDMI_OFF_TIME 2	/* 140 ms step if V2. 80 ms step if V1 */
+#define AV8100_HDMI_OFF_TIME 0	/* 140 ms step if V2. 80 ms step if V1 */
 
 /* Command offsets */
 #define AV8100_COMMAND_OFFSET		0x10
diff --git a/drivers/video/b2r2/b2r2_node_split.c b/drivers/video/b2r2/b2r2_node_split.c
index 5faca17c197..6de6db3cb37 100644
--- a/drivers/video/b2r2/b2r2_node_split.c
+++ b/drivers/video/b2r2/b2r2_node_split.c
@@ -184,7 +184,8 @@ static void configure_bg(struct b2r2_node *node,
 static int configure_dst(struct b2r2_node *node,
 		struct b2r2_node_split_buf *dst, const u32 *ivmx,
 		struct b2r2_node **next);
-static void configure_blend(struct b2r2_node *node, u32 flags, u32 global_alpha);
+static void configure_blend(struct b2r2_node *node, u32 flags,
+		u32 global_alpha);
 static void configure_clip(struct b2r2_node *node,
 		struct b2r2_blt_rect *clip_rect);
 
@@ -264,7 +265,8 @@ static void set_ivmx(struct b2r2_node *node, const u32 *vmx_values);
 
 static void reset_nodes(struct b2r2_node *node);
 
-static bool bg_format_require_ivmx(enum b2r2_blt_fmt bg_fmt, enum b2r2_blt_fmt dst_fmt);
+static bool bg_format_require_ivmx(enum b2r2_blt_fmt bg_fmt,
+		enum b2r2_blt_fmt dst_fmt);
 
 /*
  * Public functions
@@ -330,10 +332,12 @@ int b2r2_node_split_analyze(const struct b2r2_blt_request *req,
 	/* Unsupported formats on bg */
 	if (this->flags & B2R2_BLT_FLAG_BG_BLEND) {
 		/*
-		 * There are no ivmx on source 1, so check that there is no such requirement
-		 * on the background to destination format conversion. This check is sufficient
-		 * since the node splitter currently does not support destination ivmx. That
-		 * fact also removes the source format as a parameter when checking the
+		 * There are no ivmx on source 1, so check that
+		 * there is no such requirement on the background
+		 * to destination format conversion. This check is sufficient
+		 * since the node splitter currently does not support
+		 * destination ivmx. That fact also removes
+		 * the source format as a parameter when checking the
 		 * background format.
 		 */
 		if (bg_format_require_ivmx(req->user_req.bg_img.fmt,
@@ -756,7 +760,8 @@ error:
  * Check if there are any color space conversion needed for the
  * background to the destination format.
  */
-static bool bg_format_require_ivmx(enum b2r2_blt_fmt bg_fmt, enum b2r2_blt_fmt dst_fmt)
+static bool bg_format_require_ivmx(enum b2r2_blt_fmt bg_fmt,
+				enum b2r2_blt_fmt dst_fmt)
 {
 	if (is_rgb_fmt(bg_fmt)) {
 		if (is_yvu_fmt(dst_fmt))
@@ -1680,10 +1685,13 @@ static int configure_tile(struct b2r2_node_split_job *this,
 			}
 
 			if (this->blend) {
-				if (this->flags & B2R2_BLT_FLAG_BG_BLEND)
-					configure_bg(node, bg, this->swap_fg_bg);
-				else
-					configure_bg(node, dst, this->swap_fg_bg);
+				if (this->flags & B2R2_BLT_FLAG_BG_BLEND) {
+					configure_bg(node, bg,
+							this->swap_fg_bg);
+				} else {
+					configure_bg(node, dst,
+							this->swap_fg_bg);
+				}
 				configure_blend(node, this->flags,
 						this->global_alpha);
 			}
@@ -2387,6 +2395,30 @@ static int configure_scale(struct b2r2_node *node,
 			node->node.GROUP10.B2R2_RZI = luma_rzi;
 			node->node.GROUP10.B2R2_HFP = luma_hfp;
 			node->node.GROUP10.B2R2_VFP = luma_vfp;
+			/*
+			 * Scaling operation from raster to a multi-buffer
+			 * format, requires the raster input to be scaled
+			 * before luminance information can be extracted.
+			 * Raster input is scaled by the chroma resizer.
+			 * Luma resizer only handles luminance data which
+			 * exists in a separate buffer in source image,
+			 * as is the case with YUV planar/semi-planar formats.
+			 */
+			if (src_raster) {
+				/* Activate chroma scaling */
+				node->node.GROUP0.B2R2_CIC |=
+					B2R2_CIC_RESIZE_CHROMA;
+				node->node.GROUP8.B2R2_FCTL |= fctl;
+				/*
+				 * Color data must be scaled
+				 * to the same size as luma.
+				 * Use luma scaling parameters.
+				 */
+				node->node.GROUP9.B2R2_RSF = luma_rsf;
+				node->node.GROUP9.B2R2_RZI = luma_rzi;
+				node->node.GROUP9.B2R2_HFP = luma_hfp;
+				node->node.GROUP9.B2R2_VFP = luma_vfp;
+			}
 		}
 
 		b2r2_log_info("%s:\n"
@@ -2531,13 +2563,15 @@ static void configure_bg(struct b2r2_node *node,
 	case B2R2_FMT_TYPE_RASTER:
 		if (swap_fg_bg) {
 			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_2;
-			node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_SOURCE_2_FETCH_FROM_MEM;
 			node->node.GROUP0.B2R2_ACK |= B2R2_ACK_SWAP_FG_BG;
 
 			set_src(&node->node.GROUP4, bg->addr, bg);
 		} else {
 			node->node.GROUP0.B2R2_CIC |= B2R2_CIC_SOURCE_1;
-			node->node.GROUP0.B2R2_INS |= B2R2_INS_SOURCE_1_FETCH_FROM_MEM;
+			node->node.GROUP0.B2R2_INS |=
+				B2R2_INS_SOURCE_1_FETCH_FROM_MEM;
 
 			set_src(&node->node.GROUP3, bg->addr, bg);
 		}
diff --git a/fs/ext4/inode.c b/fs/ext4/inode.c
index e3126c05100..159300429d7 100644
--- a/fs/ext4/inode.c
+++ b/fs/ext4/inode.c
@@ -2148,7 +2148,12 @@ static int mpage_da_submit_io(struct mpage_da_data *mpd,
 			else if (test_opt(inode->i_sb, MBLK_IO_SUBMIT))
 				err = ext4_bio_write_page(&io_submit, page,
 							  len, mpd->wbc);
-			else
+			else if (buffer_uninit(page_bufs)) {
+				ext4_set_bh_endio(page_bufs, inode);
+				err = block_write_full_page_endio(page,
+					noalloc_get_block_write,
+					mpd->wbc, ext4_end_io_buffer_write);
+			} else
 				err = block_write_full_page(page,
 					noalloc_get_block_write, mpd->wbc);
 
diff --git a/fs/fuse/dev.c b/fs/fuse/dev.c
index 640fc229df1..99e13346a75 100644
--- a/fs/fuse/dev.c
+++ b/fs/fuse/dev.c
@@ -19,6 +19,7 @@
 #include <linux/pipe_fs_i.h>
 #include <linux/swap.h>
 #include <linux/splice.h>
+#include <linux/freezer.h>
 
 MODULE_ALIAS_MISCDEV(FUSE_MINOR);
 MODULE_ALIAS("devname:fuse");
@@ -383,7 +384,10 @@ __acquires(fc->lock)
 	 * Wait it out.
 	 */
 	spin_unlock(&fc->lock);
-	wait_event(req->waitq, req->state == FUSE_REQ_FINISHED);
+
+	while (req->state != FUSE_REQ_FINISHED)
+		wait_event_freezable(req->waitq,
+				     req->state == FUSE_REQ_FINISHED);
 	spin_lock(&fc->lock);
 
 	if (!req->aborted)
diff --git a/include/linux/input/lps001wp.h b/include/linux/input/lps001wp.h
index aa5eac9af8f..779a415ea68 100644
--- a/include/linux/input/lps001wp.h
+++ b/include/linux/input/lps001wp.h
@@ -70,8 +70,6 @@ struct lps001wp_prs_platform_data {
 
 	int (*init)(void);
 	void (*exit)(void);
-	int (*power_on)(void);
-	int (*power_off)(void);
 
 };
 
diff --git a/include/linux/ion.h b/include/linux/ion.h
index 111982f48dc..aed8349279e 100644
--- a/include/linux/ion.h
+++ b/include/linux/ion.h
@@ -186,9 +186,14 @@ void ion_unmap_dma(struct ion_client *client, struct ion_handle *handle);
  * @client:	the client
  * @handle:	the handle to share
  *
- * Given a handle, return a buffer which exists in a global name
- * space and can be passed to other clients.  Should be passed into ion_import
+ * Given a handle, return a buffer, which exists in a global name
+ * space, and can be passed to other clients.  Should be passed into ion_import
  * to obtain a new handle for this buffer.
+ *
+ * NOTE: This function does do not an extra reference.  The burden is on the
+ * caller to make sure the buffer doesn't go away while it's being passed to
+ * another client.  That is, ion_free should not be called on this handle until
+ * the buffer has been imported into the other client.
  */
 struct ion_buffer *ion_share(struct ion_client *client,
 			     struct ion_handle *handle);
diff --git a/include/linux/kernel.h b/include/linux/kernel.h
index 953352a8833..47e8dbea85c 100644
--- a/include/linux/kernel.h
+++ b/include/linux/kernel.h
@@ -736,4 +736,7 @@ extern int __build_bug_on_failed;
 # define REBUILD_DUE_TO_FTRACE_MCOUNT_RECORD
 #endif
 
+/* To identify board information in panic logs, set this */
+extern char *mach_panic_string;
+
 #endif
diff --git a/include/linux/mfd/db8500-prcmu.h b/include/linux/mfd/db8500-prcmu.h
index 7268ce92d54..41342dbad0d 100644
--- a/include/linux/mfd/db8500-prcmu.h
+++ b/include/linux/mfd/db8500-prcmu.h
@@ -11,6 +11,21 @@
 #define __MFD_DB8500_PRCMU_H
 
 #include <linux/interrupt.h>
+#include <linux/bitops.h>
+
+/*
+ * Registers
+ */
+#define DB8500_PRCM_GPIOCR 0x138
+#define DB8500_PRCM_GPIOCR_DBG_UARTMOD_CMD0	BIT(0)
+#define DB8500_PRCM_GPIOCR_DBG_STM_APE_CMD	BIT(9)
+#define DB8500_PRCM_GPIOCR_DBG_STM_MOD_CMD1	BIT(11)
+#define DB8500_PRCM_GPIOCR_SPI2_SELECT		BIT(23)
+
+#define DB8500_PRCM_DSI_SW_RESET 0x324
+#define DB8500_PRCM_DSI_SW_RESET_DSI0_SW_RESETN BIT(0)
+#define DB8500_PRCM_DSI_SW_RESET_DSI1_SW_RESETN BIT(1)
+#define DB8500_PRCM_DSI_SW_RESET_DSI2_SW_RESETN BIT(2)
 
 /* This portion previously known as <mach/prcmu-fw-defs_v1.h> */
 
@@ -519,12 +534,6 @@ int prcmu_abb_write(u8 slave, u8 reg, u8 *value, u8 size);
 void prcmu_ac_wake_req(void);
 void prcmu_ac_sleep_req(void);
 void prcmu_modem_reset(void);
-void prcmu_enable_spi2(void);
-void prcmu_disable_spi2(void);
-void prcmu_enable_stm_mod_uart(void);
-void prcmu_disable_stm_mod_uart(void);
-void prcmu_enable_stm_ape(void);
-void prcmu_disable_stm_ape(void);
 
 int prcmu_config_a9wdog(u8 num, bool sleep_auto_off);
 int prcmu_enable_a9wdog(u8 id);
@@ -552,6 +561,10 @@ int db8500_prcmu_get_ape_opp(void);
 int db8500_prcmu_set_ddr_opp(u8 opp);
 int db8500_prcmu_get_ddr_opp(void);
 
+u32 db8500_prcmu_read(unsigned int reg);
+void db8500_prcmu_write(unsigned int reg, u32 value);
+void db8500_prcmu_write_masked(unsigned int reg, u32 mask, u32 value);
+
 #else /* !CONFIG_MFD_DB8500_PRCMU */
 
 static inline void db8500_prcmu_early_init(void) {}
@@ -656,36 +669,6 @@ static inline void prcmu_ac_sleep_req(void) {}
 
 static inline void prcmu_modem_reset(void) {}
 
-static inline int prcmu_enable_spi2(void)
-{
-	return 0;
-}
-
-static inline int prcmu_disable_spi2(void)
-{
-	return 0;
-}
-
-static inline int prcmu_enable_stm_mod_uart(void)
-{
-	return 0;
-}
-
-static inline int prcmu_disable_stm_mod_uart(void)
-{
-	return 0;
-}
-
-static inline int prcmu_enable_stm_ape(void)
-{
-	return 0;
-}
-
-static inline int prcmu_disable_stm_ape(void)
-{
-	return 0;
-}
-
 static inline void db8500_prcmu_system_reset(u16 reset_code) {}
 
 static inline int db8500_prcmu_set_power_state(u8 state, bool keep_ulp_clk,
@@ -775,6 +758,16 @@ static inline int db8500_prcmu_get_arm_opp(void)
 	return 0;
 }
 
+static inline u32 db8500_prcmu_read(unsigned int reg)
+{
+	return 0;
+}
+
+static inline void db8500_prcmu_write(unsigned int reg, u32 value) {}
+
+static inline void db8500_prcmu_write_masked(unsigned int reg, u32 mask,
+	u32 value) {}
+
 #endif /* !CONFIG_MFD_DB8500_PRCMU */
 
 #endif /* __MFD_DB8500_PRCMU_H */
diff --git a/include/linux/mfd/dbx500-prcmu.h b/include/linux/mfd/dbx500-prcmu.h
index 43a62985b0a..693e7a4150a 100644
--- a/include/linux/mfd/dbx500-prcmu.h
+++ b/include/linux/mfd/dbx500-prcmu.h
@@ -464,6 +464,26 @@ static inline int prcmu_stop_temp_sense(void)
 		return  db8500_prcmu_stop_temp_sense();
 }
 
+static inline u32 prcmu_read(unsigned int reg)
+{
+	if (cpu_is_u8500())
+		return db8500_prcmu_read(reg);
+	else
+		return 0;
+}
+
+static inline void prcmu_write(unsigned int reg, u32 value)
+{
+	if (cpu_is_u8500())
+		db8500_prcmu_write(reg, value);
+}
+
+static inline void prcmu_write_masked(unsigned int reg, u32 mask, u32 value)
+{
+	if (cpu_is_u8500())
+		db8500_prcmu_write_masked(reg, mask, value);
+}
+
 #else
 
 static inline void __init prcmu_early_init(void) {}
@@ -613,6 +633,104 @@ static inline int prcmu_stop_temp_sense(void)
 	return 0;
 }
 
+static inline u32 prcmu_read(unsigned int reg)
+{
+	return 0;
+}
+
+static inline void prcmu_write(unsigned int reg, u32 value) {}
+
+static inline void prcmu_write_masked(unsigned int reg, u32 mask, u32 value) {}
+
+#endif
+
+static inline void prcmu_set(unsigned int reg, u32 bits)
+{
+	prcmu_write_masked(reg, bits, bits);
+}
+
+static inline void prcmu_clear(unsigned int reg, u32 bits)
+{
+	prcmu_write_masked(reg, bits, 0);
+}
+
+#if defined(CONFIG_UX500_SOC_DB8500) || defined(CONFIG_UX500_SOC_DB5500)
+
+/**
+ * prcmu_enable_spi2 - Enables pin muxing for SPI2 on OtherAlternateC1.
+ */
+static inline void prcmu_enable_spi2(void)
+{
+	if (cpu_is_u8500())
+		prcmu_set(DB8500_PRCM_GPIOCR, DB8500_PRCM_GPIOCR_SPI2_SELECT);
+}
+
+/**
+ * prcmu_disable_spi2 - Disables pin muxing for SPI2 on OtherAlternateC1.
+ */
+static inline void prcmu_disable_spi2(void)
+{
+	if (cpu_is_u8500())
+		prcmu_clear(DB8500_PRCM_GPIOCR, DB8500_PRCM_GPIOCR_SPI2_SELECT);
+}
+
+/**
+ * prcmu_enable_stm_mod_uart - Enables pin muxing for STMMOD
+ * and UARTMOD on OtherAlternateC3.
+ */
+static inline void prcmu_enable_stm_mod_uart(void)
+{
+	if (cpu_is_u8500()) {
+		prcmu_set(DB8500_PRCM_GPIOCR,
+			(DB8500_PRCM_GPIOCR_DBG_STM_MOD_CMD1 |
+			 DB8500_PRCM_GPIOCR_DBG_UARTMOD_CMD0));
+	}
+}
+
+/**
+ * prcmu_disable_stm_mod_uart - Disables pin muxing for STMMOD
+ * and UARTMOD on OtherAlternateC3.
+ */
+static inline void prcmu_disable_stm_mod_uart(void)
+{
+	if (cpu_is_u8500()) {
+		prcmu_clear(DB8500_PRCM_GPIOCR,
+			(DB8500_PRCM_GPIOCR_DBG_STM_MOD_CMD1 |
+			 DB8500_PRCM_GPIOCR_DBG_UARTMOD_CMD0));
+	}
+}
+
+/**
+ * prcmu_enable_stm_ape - Enables pin muxing for STM APE on OtherAlternateC1.
+ */
+static inline void prcmu_enable_stm_ape(void)
+{
+	if (cpu_is_u8500()) {
+		prcmu_set(DB8500_PRCM_GPIOCR,
+			DB8500_PRCM_GPIOCR_DBG_STM_APE_CMD);
+	}
+}
+
+/**
+ * prcmu_disable_stm_ape - Disables pin muxing for STM APE on OtherAlternateC1.
+ */
+static inline void prcmu_disable_stm_ape(void)
+{
+	if (cpu_is_u8500()) {
+		prcmu_clear(DB8500_PRCM_GPIOCR,
+			DB8500_PRCM_GPIOCR_DBG_STM_APE_CMD);
+	}
+}
+
+#else
+
+static inline void prcmu_enable_spi2(void) {}
+static inline void prcmu_disable_spi2(void) {}
+static inline void prcmu_enable_stm_mod_uart(void) {}
+static inline void prcmu_disable_stm_mod_uart(void) {}
+static inline void prcmu_enable_stm_ape(void) {}
+static inline void prcmu_disable_stm_ape(void) {}
+
 #endif
 
 /* PRCMU QoS APE OPP class */
diff --git a/include/linux/mmc/card.h b/include/linux/mmc/card.h
index f56ffc70382..b072bce633f 100644
--- a/include/linux/mmc/card.h
+++ b/include/linux/mmc/card.h
@@ -213,6 +213,7 @@ struct mmc_card {
 #define MMC_QUIRK_INAND_CMD38	(1<<6)		/* iNAND devices have broken CMD38 */
 #define MMC_QUIRK_BLK_NO_CMD23	(1<<7)		/* Avoid CMD23 for regular multiblock */
 #define MMC_QUIRK_BROKEN_BYTE_MODE_512 (1<<8)	/* Avoid sending 512 bytes in byte mode */
+#define MMC_QUIRK_LONG_READ_TIME (1<<9)		/* Data read time > CSD says */
 
 	unsigned int		erase_size;	/* erase size in sectors */
  	unsigned int		erase_shift;	/* if erase unit is power 2 */
@@ -424,6 +425,11 @@ static inline int mmc_card_broken_byte_mode_512(const struct mmc_card *c)
 	return c->quirks & MMC_QUIRK_BROKEN_BYTE_MODE_512;
 }
 
+static inline int mmc_card_long_read_time(const struct mmc_card *c)
+{
+	return c->quirks & MMC_QUIRK_LONG_READ_TIME;
+}
+
 #define mmc_card_name(c)	((c)->cid.prod_name)
 #define mmc_card_id(c)		(dev_name(&(c)->dev))
 
diff --git a/include/linux/usb/gadget.h b/include/linux/usb/gadget.h
index dd1571db55e..5802cb09fda 100644
--- a/include/linux/usb/gadget.h
+++ b/include/linux/usb/gadget.h
@@ -430,6 +430,8 @@ struct usb_gadget_ops {
 	int	(*pullup) (struct usb_gadget *, int is_on);
 	int	(*ioctl)(struct usb_gadget *,
 				unsigned code, unsigned long param);
+	struct usb_ep* (*configure_ep)(struct usb_gadget *, u8 type,
+				struct usb_endpoint_descriptor *);
 };
 
 /**
diff --git a/include/linux/usb/otg_id.h b/include/linux/usb/otg_id.h
index b686ab06795..46a44637c11 100644
--- a/include/linux/usb/otg_id.h
+++ b/include/linux/usb/otg_id.h
@@ -28,6 +28,10 @@
  *    get called first.
  * @detect: Called during otg_id_notify.  Return OTG_ID_HANDLED if the USB cable
  *    has been identified
+ * @proxy_wait: Called during otg_id_notify if a previous handler returns
+ *    OTG_ID_PROXY_WAIT.  This should wait on ID change then call otg_id_notify.
+ *    This is used when a handler knows what's connected but can't detect
+ *    the change itself.
  * @cancel: Called after detect has returned OTG_ID_HANDLED to ask it to
  *    release detection resources to allow a new identification to occur.
  */
@@ -35,10 +39,12 @@
 struct otg_id_notifier_block {
 	int priority;
 	int (*detect)(struct otg_id_notifier_block *otg_id_nb);
+	int (*proxy_wait)(struct otg_id_notifier_block *otg_id_nb);
 	void (*cancel)(struct otg_id_notifier_block *otg_id_nb);
 	struct plist_node p;
 };
 
+#define OTG_ID_PROXY_WAIT 2
 #define OTG_ID_HANDLED 1
 #define OTG_ID_UNHANDLED 0
 
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 578545a2a49..8e85fb32a5f 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -39,8 +39,11 @@
 #define L2CAP_DEFAULT_ACK_TO		200
 #define L2CAP_LE_DEFAULT_MTU		23
 
-#define L2CAP_CONN_TIMEOUT	(40000) /* 40 seconds */
-#define L2CAP_INFO_TIMEOUT	(4000)  /*  4 seconds */
+#define L2CAP_DISC_TIMEOUT		(100)
+#define L2CAP_DISC_REJ_TIMEOUT		(5000)  /*  5 seconds */
+#define L2CAP_ENC_TIMEOUT		(5000)  /*  5 seconds */
+#define L2CAP_CONN_TIMEOUT		(40000) /* 40 seconds */
+#define L2CAP_INFO_TIMEOUT		(4000)  /*  4 seconds */
 
 /* L2CAP socket address */
 struct sockaddr_l2 {
diff --git a/kernel/panic.c b/kernel/panic.c
index 5578d0adc27..a136da2f396 100644
--- a/kernel/panic.c
+++ b/kernel/panic.c
@@ -27,6 +27,9 @@
 #define PANIC_TIMER_STEP 100
 #define PANIC_BLINK_SPD 18
 
+/* Machine specific panic information string */
+char *mach_panic_string;
+
 int panic_on_oops;
 static unsigned long tainted_mask;
 static int pause_on_oops;
@@ -345,6 +348,11 @@ late_initcall(init_oops_id);
 void print_oops_end_marker(void)
 {
 	init_oops_id();
+
+	if (mach_panic_string)
+		printk(KERN_WARNING "Board Information: %s\n",
+		       mach_panic_string);
+
 	printk(KERN_WARNING "---[ end trace %016llx ]---\n",
 		(unsigned long long)oops_id);
 }
diff --git a/kernel/power/wakelock.c b/kernel/power/wakelock.c
index c10d0ee7907..2ee459fe445 100644
--- a/kernel/power/wakelock.c
+++ b/kernel/power/wakelock.c
@@ -249,7 +249,7 @@ long has_wake_lock(int type)
 	unsigned long irqflags;
 	spin_lock_irqsave(&list_lock, irqflags);
 	ret = has_wake_lock_locked(type);
-	if (ret && (debug_mask & DEBUG_SUSPEND) && type == WAKE_LOCK_SUSPEND)
+	if (ret && (debug_mask & DEBUG_WAKEUP) && type == WAKE_LOCK_SUSPEND)
 		print_active_locks(type);
 	spin_unlock_irqrestore(&list_lock, irqflags);
 	return ret;
@@ -311,7 +311,7 @@ static int power_suspend_late(struct device *dev)
 {
 	int ret = has_wake_lock(WAKE_LOCK_SUSPEND) ? -EAGAIN : 0;
 #ifdef CONFIG_WAKELOCK_STAT
-	wait_for_wakeup = 1;
+	wait_for_wakeup = !ret;
 #endif
 	if (debug_mask & DEBUG_SUSPEND)
 		pr_info("power_suspend_late return %d\n", ret);
diff --git a/net/bluetooth/bnep/bnep.h b/net/bluetooth/bnep/bnep.h
index 449508aeb01..0545fe0493f 100644
--- a/net/bluetooth/bnep/bnep.h
+++ b/net/bluetooth/bnep/bnep.h
@@ -155,6 +155,7 @@ struct bnep_session {
 	unsigned int  role;
 	unsigned long state;
 	unsigned long flags;
+	atomic_t      terminate;
 	struct task_struct *task;
 
 	struct ethhdr eh;
diff --git a/net/bluetooth/bnep/core.c b/net/bluetooth/bnep/core.c
index e1c5c1b13d3..b7026f35a9b 100644
--- a/net/bluetooth/bnep/core.c
+++ b/net/bluetooth/bnep/core.c
@@ -522,7 +522,7 @@ static int bnep_session(void *arg)
 	while (1) {
 		set_current_state(TASK_INTERRUPTIBLE);
 
-		if (kthread_should_stop())
+		if (atomic_read(&s->terminate))
 			break;
 		/* RX */
 		while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
@@ -677,9 +677,10 @@ int bnep_del_connection(struct bnep_conndel_req *req)
 	down_read(&bnep_session_sem);
 
 	s = __bnep_get_session(req->dst);
-	if (s)
-		kthread_stop(s->task);
-	else
+	if (s) {
+		atomic_inc(&s->terminate);
+		wake_up_process(s->task);
+	} else
 		err = -ENOENT;
 
 	up_read(&bnep_session_sem);
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 40248744a86..aecb1e7fdfa 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -631,6 +631,10 @@ static int hci_conn_auth(struct hci_conn *conn, __u8 sec_level, __u8 auth_type)
 
 	if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->pend)) {
 		struct hci_cp_auth_requested cp;
+
+		/* encrypt must be pending if auth is also pending */
+		set_bit(HCI_CONN_ENCRYPT_PEND, &conn->pend);
+
 		cp.handle = cpu_to_le16(conn->handle);
 		hci_send_cmd(conn->hdev, HCI_OP_AUTH_REQUESTED,
 							sizeof(cp), &cp);
@@ -696,7 +700,7 @@ int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type)
 		goto encrypt;
 
 auth:
-	if (test_bit(HCI_CONN_ENCRYPT_PEND, &conn->pend))
+	if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &conn->pend))
 		return 0;
 
 	if (!hci_conn_auth(conn, sec_level, auth_type))
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 908fcd384ab..3b391986407 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -1209,7 +1209,6 @@ static void hci_cmd_timer(unsigned long arg)
 
 	BT_ERR("%s command tx timeout", hdev->name);
 	atomic_set(&hdev->cmd_cnt, 1);
-	clear_bit(HCI_RESET, &hdev->flags);
 	tasklet_schedule(&hdev->cmd_task);
 }
 
@@ -2408,7 +2407,10 @@ static void hci_cmd_task(unsigned long arg)
 		if (hdev->sent_cmd) {
 			atomic_dec(&hdev->cmd_cnt);
 			hci_send_frame(skb);
-			mod_timer(&hdev->cmd_timer,
+			if (test_bit(HCI_RESET, &hdev->flags))
+				del_timer(&hdev->cmd_timer);
+			else
+				mod_timer(&hdev->cmd_timer,
 				  jiffies + msecs_to_jiffies(HCI_CMD_TIMEOUT));
 		} else {
 			skb_queue_head(&hdev->cmd_q, skb);
diff --git a/net/bluetooth/hidp/core.c b/net/bluetooth/hidp/core.c
index 43b4c2deb7c..fb68f344c34 100644
--- a/net/bluetooth/hidp/core.c
+++ b/net/bluetooth/hidp/core.c
@@ -764,6 +764,7 @@ static int hidp_session(void *arg)
 
 	up_write(&hidp_session_sem);
 
+	kfree(session->rd_data);
 	kfree(session);
 	return 0;
 }
@@ -841,7 +842,8 @@ static int hidp_setup_input(struct hidp_session *session,
 
 	err = input_register_device(input);
 	if (err < 0) {
-		hci_conn_put_device(session->conn);
+		input_free_device(input);
+		session->input = NULL;
 		return err;
 	}
 
@@ -1044,8 +1046,12 @@ int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock,
 	}
 
 	err = hid_add_device(session->hid);
-	if (err < 0)
-		goto err_add_device;
+	if (err < 0) {
+		atomic_inc(&session->terminate);
+		wake_up_process(session->task);
+		up_write(&hidp_session_sem);
+		return err;
+	}
 
 	if (session->input) {
 		hidp_send_ctrl_message(session,
@@ -1059,12 +1065,6 @@ int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock,
 	up_write(&hidp_session_sem);
 	return 0;
 
-err_add_device:
-	hid_destroy_device(session->hid);
-	session->hid = NULL;
-	atomic_inc(&session->terminate);
-	wake_up_process(session->task);
-
 unlink:
 	hidp_del_timer(session);
 
@@ -1090,7 +1090,6 @@ purge:
 failed:
 	up_write(&hidp_session_sem);
 
-	input_free_device(session->input);
 	kfree(session);
 	return err;
 }
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index dc23b89d81b..6ec11b06ca2 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -251,7 +251,7 @@ static void l2cap_chan_timeout(unsigned long arg)
 
 	if (sock_owned_by_user(sk)) {
 		/* sk is owned by user. Try again later */
-		__set_chan_timer(chan, HZ / 5);
+		__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 		bh_unlock_sock(sk);
 		chan_put(chan);
 		return;
@@ -2481,7 +2481,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
 		if (sock_owned_by_user(sk)) {
 			l2cap_state_change(chan, BT_DISCONN);
 			__clear_chan_timer(chan);
-			__set_chan_timer(chan, HZ / 5);
+			__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			break;
 		}
 
@@ -2651,7 +2651,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
 	default:
 		sk->sk_err = ECONNRESET;
-		__set_chan_timer(chan, HZ * 5);
+		__set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
 		l2cap_send_disconn_req(conn, chan, ECONNRESET);
 		goto done;
 	}
@@ -2708,7 +2708,7 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
 	if (sock_owned_by_user(sk)) {
 		l2cap_state_change(chan, BT_DISCONN);
 		__clear_chan_timer(chan);
-		__set_chan_timer(chan, HZ / 5);
+		__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 		bh_unlock_sock(sk);
 		return 0;
 	}
@@ -2742,7 +2742,7 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
 	if (sock_owned_by_user(sk)) {
 		l2cap_state_change(chan,BT_DISCONN);
 		__clear_chan_timer(chan);
-		__set_chan_timer(chan, HZ / 5);
+		__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 		bh_unlock_sock(sk);
 		return 0;
 	}
@@ -4071,7 +4071,7 @@ static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
 	if (encrypt == 0x00) {
 		if (chan->sec_level == BT_SECURITY_MEDIUM) {
 			__clear_chan_timer(chan);
-			__set_chan_timer(chan, HZ * 5);
+			__set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
 		} else if (chan->sec_level == BT_SECURITY_HIGH)
 			l2cap_chan_close(chan, ECONNREFUSED);
 	} else {
@@ -4136,7 +4136,7 @@ static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 					L2CAP_CONN_REQ, sizeof(req), &req);
 			} else {
 				__clear_chan_timer(chan);
-				__set_chan_timer(chan, HZ / 10);
+				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -4156,7 +4156,7 @@ static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 				}
 			} else {
 				l2cap_state_change(chan, BT_DISCONN);
-				__set_chan_timer(chan, HZ / 10);
+				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 				res = L2CAP_CR_SEC_BLOCK;
 				stat = L2CAP_CS_NO_INFO;
 			}
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 7d713b1c4cb..61f1f623091 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -993,7 +993,7 @@ static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int p
 	INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
 
 	sk->sk_destruct = l2cap_sock_destruct;
-	sk->sk_sndtimeo = msecs_to_jiffies(L2CAP_CONN_TIMEOUT);
+	sk->sk_sndtimeo = L2CAP_CONN_TIMEOUT;
 
 	sock_reset_flag(sk, SOCK_ZAPPED);
 
diff --git a/net/netfilter/xt_qtaguid.c b/net/netfilter/xt_qtaguid.c
index 22552c9b81c..80b5990045b 100644
--- a/net/netfilter/xt_qtaguid.c
+++ b/net/netfilter/xt_qtaguid.c
@@ -8,8 +8,36 @@
  * published by the Free Software Foundation.
  */
 
-/* TODO: support ipv6 for iface_stat.
- * Currently if an iface is only v6 it will not have stats collected. */
+/* #define DEBUG */
+/* #define IDEBUG */
+/* #define MDEBUG */
+/* #define RDEBUG */
+/* #define CDEBUG */
+
+/* Iface handling */
+#ifdef IDEBUG
+#define IF_DEBUG(...) pr_debug(__VA_ARGS__)
+#else
+#define IF_DEBUG(...) no_printk(__VA_ARGS__)
+#endif
+/* Iptable Matching */
+#ifdef MDEBUG
+#define MT_DEBUG(...) pr_debug(__VA_ARGS__)
+#else
+#define MT_DEBUG(...) no_printk(__VA_ARGS__)
+#endif
+/* Red-black tree handling */
+#ifdef RDEBUG
+#define RB_DEBUG(...) pr_debug(__VA_ARGS__)
+#else
+#define RB_DEBUG(...) no_printk(__VA_ARGS__)
+#endif
+/* procfs ctrl/stats handling */
+#ifdef CDEBUG
+#define CT_DEBUG(...) pr_debug(__VA_ARGS__)
+#else
+#define CT_DEBUG(...) no_printk(__VA_ARGS__)
+#endif
 
 #include <linux/file.h>
 #include <linux/inetdevice.h>
@@ -18,13 +46,16 @@
 #include <linux/netfilter/xt_qtaguid.h>
 #include <linux/skbuff.h>
 #include <linux/workqueue.h>
+#include <net/addrconf.h>
 #include <net/sock.h>
 #include <net/tcp.h>
 #include <net/udp.h>
 
 #include <linux/netfilter/xt_socket.h>
-/* We only use the xt_socket funcs within a similar context to avoid unexpected
- * return values. */
+/*
+ * We only use the xt_socket funcs within a similar context to avoid unexpected
+ * return values.
+ */
 #define XT_SOCKET_SUPPORTED_HOOKS \
 	((1 << NF_INET_PRE_ROUTING) | (1 << NF_INET_LOCAL_IN))
 
@@ -61,7 +92,8 @@ module_param_named(stats_readall_gid, proc_stats_readall_gid, uint,
 module_param_named(ctrl_write_gid, proc_ctrl_write_gid, uint,
 		   S_IRUGO | S_IWUSR);
 
-/* After the kernel has initiallized this module, it is still possible
+/*
+ * After the kernel has initiallized this module, it is still possible
  * to make it passive:
  *  - do not register it via iptables.
  *   the matching code will not be invoked.
@@ -106,6 +138,14 @@ typedef uint64_t tag_t;  /* Only used via accessors */
 static const char *iface_stat_procdirname = "iface_stat";
 static struct proc_dir_entry *iface_stat_procdir;
 
+
+/*
+ * For now we only track 2 sets of counters.
+ * The default set is 0.
+ * Userspace can activate another set for a given uid being tracked.
+ */
+#define IFS_MAX_COUNTER_SETS 2
+
 enum ifs_tx_rx {
 	IFS_TX,
 	IFS_RX,
@@ -126,18 +166,23 @@ struct byte_packet_counters {
 };
 
 struct data_counters {
-	struct byte_packet_counters bpc[IFS_MAX_DIRECTIONS][IFS_MAX_PROTOS];
+	struct byte_packet_counters bpc[IFS_MAX_COUNTER_SETS][IFS_MAX_DIRECTIONS][IFS_MAX_PROTOS];
 };
 
-struct tag_stat {
+/* Generic tag based node used as a base for rb_tree ops. */
+struct tag_node {
 	struct rb_node node;
 	tag_t tag;
+};
 
+struct tag_stat {
+	struct tag_node tn;
 	struct data_counters counters;
-	/* If this tag is acct_tag based, we need to count against the
-	 * matching parent uid_tag. */
+	/*
+	 * If this tag is acct_tag based, we need to count against the
+	 * matching parent uid_tag.
+	 */
 	struct data_counters *parent_counters;
-	struct proc_dir_entry *proc_ptr;
 };
 
 struct iface_stat {
@@ -157,20 +202,60 @@ struct iface_stat {
 static LIST_HEAD(iface_stat_list);
 static DEFINE_SPINLOCK(iface_stat_list_lock);
 
+/* This is needed to create proc_dir_entries from atomic context. */
+struct iface_stat_work {
+	struct work_struct iface_work;
+	struct iface_stat *iface_entry;
+};
+
 /*
- * Track tag that this socket is transferring data for, and not necesseraly
+ * Track tag that this socket is transferring data for, and not necessarily
  * the uid that owns the socket.
  * This is the tag against which tag_stat.counters will be billed.
  */
 struct sock_tag {
-	struct rb_node node;
-	struct sock *sk;
+	struct rb_node sock_node;
+	struct sock *sk;  /* Only used as a number, never dereferenced */
+	/* The socket is needed for sockfd_put() */
+	struct socket *socket;
+
 	tag_t tag;
 };
 
+struct qtaguid_event_counts {
+	/* Various successful events */
+	atomic64_t sockets_tagged;
+	atomic64_t sockets_untagged;
+	atomic64_t counter_set_changes;
+	atomic64_t delete_cmds;
+	atomic64_t iface_events;  /* Number of NETDEV_* events handled */
+	/*
+	 * match_found_sk_*: numbers related to the netfilter matching
+	 * function finding a sock for the sk_buff.
+	 */
+	atomic64_t match_found_sk;   /* An sk was already in the sk_buff. */
+	/* The connection tracker had the sk. */
+	atomic64_t match_found_sk_in_ct;
+	/*
+	 * No sk could be found. No apparent owner. Could happen with
+	 * unsolicited traffic.
+	 */
+	atomic64_t match_found_sk_none;
+};
+static struct qtaguid_event_counts qtu_events;
+
 static struct rb_root sock_tag_tree = RB_ROOT;
 static DEFINE_SPINLOCK(sock_tag_list_lock);
 
+/* Track the set active_set for the given tag. */
+struct tag_counter_set {
+	struct tag_node tn;
+	int active_set;
+};
+
+static struct rb_root tag_counter_set_tree = RB_ROOT;
+static DEFINE_SPINLOCK(tag_counter_set_list_lock);
+
 static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par);
 
 /*----------------------------------------------*/
@@ -179,7 +264,6 @@ static inline int tag_compare(tag_t t1, tag_t t2)
 	return t1 < t2 ? -1 : t1 == t2 ? 0 : 1;
 }
 
-
 static inline tag_t combine_atag_with_uid(tag_t acct_tag, uid_t uid)
 {
 	return acct_tag | uid;
@@ -206,40 +290,42 @@ static inline bool valid_atag(tag_t tag)
 	return !(tag & 0xFFFFFFFFULL);
 }
 
-static inline void dc_add_byte_packets(struct data_counters *counters,
+static inline void dc_add_byte_packets(struct data_counters *counters, int set,
 				  enum ifs_tx_rx direction,
 				  enum ifs_proto ifs_proto,
 				  int bytes,
 				  int packets)
 {
-	counters->bpc[direction][ifs_proto].bytes += bytes;
-	counters->bpc[direction][ifs_proto].packets += packets;
+	counters->bpc[set][direction][ifs_proto].bytes += bytes;
+	counters->bpc[set][direction][ifs_proto].packets += packets;
 }
 
 static inline uint64_t dc_sum_bytes(struct data_counters *counters,
+				    int set,
 				    enum ifs_tx_rx direction)
 {
-	return counters->bpc[direction][IFS_TCP].bytes
-		+ counters->bpc[direction][IFS_UDP].bytes
-		+ counters->bpc[direction][IFS_PROTO_OTHER].bytes;
+	return counters->bpc[set][direction][IFS_TCP].bytes
+		+ counters->bpc[set][direction][IFS_UDP].bytes
+		+ counters->bpc[set][direction][IFS_PROTO_OTHER].bytes;
 }
 
 static inline uint64_t dc_sum_packets(struct data_counters *counters,
+				      int set,
 				      enum ifs_tx_rx direction)
 {
-	return counters->bpc[direction][IFS_TCP].packets
-		+ counters->bpc[direction][IFS_UDP].packets
-		+ counters->bpc[direction][IFS_PROTO_OTHER].packets;
+	return counters->bpc[set][direction][IFS_TCP].packets
+		+ counters->bpc[set][direction][IFS_UDP].packets
+		+ counters->bpc[set][direction][IFS_PROTO_OTHER].packets;
 }
 
-static struct tag_stat *tag_stat_tree_search(struct rb_root *root, tag_t tag)
+static struct tag_node *tag_node_tree_search(struct rb_root *root, tag_t tag)
 {
 	struct rb_node *node = root->rb_node;
 
 	while (node) {
-		struct tag_stat *data = rb_entry(node, struct tag_stat, node);
+		struct tag_node *data = rb_entry(node, struct tag_node, node);
 		int result = tag_compare(tag, data->tag);
-		pr_debug("qtaguid: tag_stat_tree_search(): tag=0x%llx"
+		RB_DEBUG("qtaguid: tag_node_tree_search(): tag=0x%llx"
 			 " (uid=%d)\n",
 			 data->tag,
 			 get_uid_from_tag(data->tag));
@@ -254,16 +340,16 @@ static struct tag_stat *tag_stat_tree_search(struct rb_root *root, tag_t tag)
 	return NULL;
 }
 
-static void tag_stat_tree_insert(struct tag_stat *data, struct rb_root *root)
+static void tag_node_tree_insert(struct tag_node *data, struct rb_root *root)
 {
 	struct rb_node **new = &(root->rb_node), *parent = NULL;
 
 	/* Figure out where to put new node */
 	while (*new) {
-		struct tag_stat *this = rb_entry(*new, struct tag_stat,
+		struct tag_node *this = rb_entry(*new, struct tag_node,
 						 node);
 		int result = tag_compare(data->tag, this->tag);
-		pr_debug("qtaguid: tag_stat_tree_insert(): tag=0x%llx"
+		RB_DEBUG("qtaguid: tag_node_tree_insert(): tag=0x%llx"
 			 " (uid=%d)\n",
 			 this->tag,
 			 get_uid_from_tag(this->tag));
@@ -281,13 +367,43 @@ static void tag_stat_tree_insert(struct tag_stat *data, struct rb_root *root)
 	rb_insert_color(&data->node, root);
 }
 
+static void tag_stat_tree_insert(struct tag_stat *data, struct rb_root *root)
+{
+	tag_node_tree_insert(&data->tn, root);
+}
+
+static struct tag_stat *tag_stat_tree_search(struct rb_root *root, tag_t tag)
+{
+	struct tag_node *node = tag_node_tree_search(root, tag);
+	if (!node)
+		return NULL;
+	return rb_entry(&node->node, struct tag_stat, tn.node);
+}
+
+static void tag_counter_set_tree_insert(struct tag_counter_set *data,
+					struct rb_root *root)
+{
+	tag_node_tree_insert(&data->tn, root);
+}
+
+static struct tag_counter_set *tag_counter_set_tree_search(struct rb_root *root,
+							   tag_t tag)
+{
+	struct tag_node *node = tag_node_tree_search(root, tag);
+	if (!node)
+		return NULL;
+	return rb_entry(&node->node, struct tag_counter_set, tn.node);
+
+}
+
 static struct sock_tag *sock_tag_tree_search(struct rb_root *root,
 					     const struct sock *sk)
 {
 	struct rb_node *node = root->rb_node;
 
 	while (node) {
-		struct sock_tag *data = rb_entry(node, struct sock_tag, node);
+		struct sock_tag *data = rb_entry(node, struct sock_tag,
+						 sock_node);
 		ptrdiff_t result = sk - data->sk;
 		if (result < 0)
 			node = node->rb_left;
@@ -305,7 +421,8 @@ static void sock_tag_tree_insert(struct sock_tag *data, struct rb_root *root)
 
 	/* Figure out where to put new node */
 	while (*new) {
-		struct sock_tag *this = rb_entry(*new, struct sock_tag, node);
+		struct sock_tag *this = rb_entry(*new, struct sock_tag,
+						 sock_node);
 		ptrdiff_t result = data->sk - this->sk;
 		parent = *new;
 		if (result < 0)
@@ -317,8 +434,8 @@ static void sock_tag_tree_insert(struct sock_tag *data, struct rb_root *root)
 	}
 
 	/* Add new node and rebalance tree. */
-	rb_link_node(&data->node, parent, new);
-	rb_insert_color(&data->node, root);
+	rb_link_node(&data->sock_node, parent, new);
+	rb_insert_color(&data->sock_node, root);
 }
 
 static int read_proc_u64(char *page, char **start, off_t off,
@@ -328,6 +445,7 @@ static int read_proc_u64(char *page, char **start, off_t off,
 	uint64_t value;
 	char *p = page;
 	uint64_t *iface_entry = data;
+
 	if (!data)
 		return 0;
 
@@ -346,6 +464,7 @@ static int read_proc_bool(char *page, char **start, off_t off,
 	bool value;
 	char *p = page;
 	bool *bool_entry = data;
+
 	if (!data)
 		return 0;
 
@@ -357,265 +476,387 @@ static int read_proc_bool(char *page, char **start, off_t off,
 	return len;
 }
 
-/* Find the entry for tracking the specified interface. */
-static struct iface_stat *get_iface_stat(const char *ifname)
+static int get_active_counter_set(tag_t tag)
+{
+	int active_set = 0;
+	struct tag_counter_set *tcs;
+
+	MT_DEBUG("qtaguid: get_active_counter_set(tag=0x%llx)"
+		 " (uid=%u)\n",
+		 tag, get_uid_from_tag(tag));
+	/* For now we only handle UID tags for active sets */
+	tag = get_utag_from_tag(tag);
+	spin_lock_bh(&tag_counter_set_list_lock);
+	tcs = tag_counter_set_tree_search(&tag_counter_set_tree, tag);
+	if (tcs)
+		active_set = tcs->active_set;
+	spin_unlock_bh(&tag_counter_set_list_lock);
+	return active_set;
+}
+
+/*
+ * Find the entry for tracking the specified interface.
+ * Caller must hold iface_stat_list_lock
+ */
+static struct iface_stat *get_iface_entry(const char *ifname)
 {
-	unsigned long flags;
 	struct iface_stat *iface_entry;
-	if (!ifname)
+
+	/* Find the entry for tracking the specified tag within the interface */
+	if (ifname == NULL) {
+		pr_info("qtaguid: iface_stat: get() NULL device name\n");
 		return NULL;
+	}
 
-	spin_lock_irqsave(&iface_stat_list_lock, flags);
+	/* Iterate over interfaces */
 	list_for_each_entry(iface_entry, &iface_stat_list, list) {
-		if (!strcmp(iface_entry->ifname, ifname))
+		if (!strcmp(ifname, iface_entry->ifname))
 			goto done;
 	}
 	iface_entry = NULL;
 done:
-	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
 	return iface_entry;
 }
 
+static void iface_create_proc_worker(struct work_struct *work)
+{
+	struct proc_dir_entry *proc_entry;
+	struct iface_stat_work *isw = container_of(work, struct iface_stat_work,
+						   iface_work);
+	struct iface_stat *new_iface  = isw->iface_entry;
+
+	/* iface_entries are not deleted, so safe to manipulate. */
+	proc_entry = proc_mkdir(new_iface->ifname, iface_stat_procdir);
+	if (IS_ERR_OR_NULL(proc_entry)) {
+		pr_err("qtaguid: iface_stat: create_proc(): alloc failed.\n");
+		kfree(isw);
+		return;
+	}
+
+	new_iface->proc_ptr = proc_entry;
+
+	create_proc_read_entry("tx_bytes", proc_iface_perms, proc_entry,
+			read_proc_u64, &new_iface->tx_bytes);
+	create_proc_read_entry("rx_bytes", proc_iface_perms, proc_entry,
+			read_proc_u64, &new_iface->rx_bytes);
+	create_proc_read_entry("tx_packets", proc_iface_perms, proc_entry,
+			read_proc_u64, &new_iface->tx_packets);
+	create_proc_read_entry("rx_packets", proc_iface_perms, proc_entry,
+			read_proc_u64, &new_iface->rx_packets);
+	create_proc_read_entry("active", proc_iface_perms, proc_entry,
+			read_proc_bool, &new_iface->active);
+
+	IF_DEBUG("qtaguid: iface_stat: create_proc(): done "
+		 "entry=%p dev=%s\n", new_iface, new_iface->ifname);
+	kfree(isw);
+}
+
+/* Caller must hold iface_stat_list_lock */
+static struct iface_stat *iface_alloc(const char *ifname)
+{
+	struct iface_stat *new_iface;
+	struct iface_stat_work *isw;
+
+	new_iface = kzalloc(sizeof(*new_iface), GFP_ATOMIC);
+	if (new_iface == NULL) {
+		pr_err("qtaguid: iface_stat: create(%s): "
+		       "iface_stat alloc failed\n", ifname);
+		return NULL;
+	}
+	new_iface->ifname = kstrdup(ifname, GFP_ATOMIC);
+	if (new_iface->ifname == NULL) {
+		pr_err("qtaguid: iface_stat: create(%s): "
+		       "ifname alloc failed\n", ifname);
+		kfree(new_iface);
+		return NULL;
+	}
+	spin_lock_init(&new_iface->tag_stat_list_lock);
+	new_iface->active = true;
+	new_iface->tag_stat_tree = RB_ROOT;
+
+	/*
+	 * ipv6 notifier chains are atomic :(
+	 * No create_proc_read_entry() for you!
+	 */
+	isw = kmalloc(sizeof(*isw), GFP_ATOMIC);
+	if (!isw) {
+		pr_err("qtaguid: iface_stat: create(%s): "
+		       "work alloc failed\n", new_iface->ifname);
+		kfree(new_iface->ifname);
+		kfree(new_iface);
+		return NULL;
+	}
+	isw->iface_entry = new_iface;
+	INIT_WORK(&isw->iface_work, iface_create_proc_worker);
+	schedule_work(&isw->iface_work);
+	list_add(&new_iface->list, &iface_stat_list);
+	return new_iface;
+}
+
 /*
  * Create a new entry for tracking the specified interface.
  * Do nothing if the entry already exists.
  * Called when an interface is configured with a valid IP address.
  */
-void iface_stat_create(const struct net_device *net_dev)
+void iface_stat_create(const struct net_device *net_dev,
+		       struct in_ifaddr *ifa)
 {
-	struct in_device *in_dev;
-	unsigned long flags;
-	struct iface_stat *new_iface;
-	struct proc_dir_entry *proc_entry;
+	struct in_device *in_dev = NULL;
 	const char *ifname;
 	struct iface_stat *entry;
 	__be32 ipaddr = 0;
-	struct in_ifaddr *ifa = NULL;
-
-	ASSERT_RTNL(); /* No need for separate locking */
+	struct iface_stat *new_iface;
 
-	pr_debug("iface_stat: create(): netdev=%p->name=%s\n",
-		 net_dev, net_dev ? net_dev->name : "");
+	IF_DEBUG("qtaguid: iface_stat: create(%s): ifa=%p netdev=%p\n",
+		 net_dev ? net_dev->name : "?",
+		 ifa, net_dev);
 	if (!net_dev) {
-		pr_err("iface_stat: create(): no net dev!\n");
+		pr_err("qtaguid: iface_stat: create(): no net dev\n");
 		return;
 	}
 
-	in_dev = __in_dev_get_rtnl(net_dev);
-	if (!in_dev) {
-		pr_err("iface_stat: create(): no inet dev!\n");
-		return;
+	ifname = net_dev->name;
+	if (!ifa) {
+		in_dev = in_dev_get(net_dev);
+		if (!in_dev) {
+			pr_err("qtaguid: iface_stat: create(%s): no inet dev\n",
+			       ifname);
+			return;
+		}
+		IF_DEBUG("qtaguid: iface_stat: create(%s): in_dev=%p\n",
+			 ifname, in_dev);
+		for (ifa = in_dev->ifa_list; ifa; ifa = ifa->ifa_next) {
+			IF_DEBUG("qtaguid: iface_stat: create(%s): "
+				 "ifa=%p ifa_label=%s\n",
+				 ifname, ifa,
+				 ifa->ifa_label ? ifa->ifa_label : "(null)");
+			if (ifa->ifa_label && !strcmp(ifname, ifa->ifa_label))
+				break;
+		}
 	}
 
-	pr_debug("iface_stat: create(): in_dev=%p\n", in_dev);
-	ifname = net_dev->name;
-	pr_debug("iface_stat: create(): ifname=%p\n", ifname);
-	for (ifa = in_dev->ifa_list; ifa; ifa = ifa->ifa_next) {
-		pr_debug("iface_stat: create(): for(): ifa=%p ifname=%p\n",
-			 ifa, ifname);
-		pr_debug("iface_stat: create(): ifname=%s ifa_label=%s\n",
-			 ifname, ifa->ifa_label ? ifa->ifa_label : "(null)");
-		if (ifa->ifa_label && !strcmp(ifname, ifa->ifa_label))
-			break;
-	}
-
-	if (ifa) {
-		ipaddr = ifa->ifa_local;
-	} else {
-		pr_err("iface_stat: create(): dev %s has no matching IP\n",
-		       ifname);
-		return;
+	if (!ifa) {
+		IF_DEBUG("qtaguid: iface_stat: create(%s): no matching IP\n",
+			 ifname);
+		goto done_put;
 	}
+	ipaddr = ifa->ifa_local;
 
-	entry = get_iface_stat(net_dev->name);
+	spin_lock_bh(&iface_stat_list_lock);
+	entry = get_iface_entry(ifname);
 	if (entry != NULL) {
-		pr_debug("iface_stat: create(): dev %s entry=%p\n", ifname,
-			 entry);
+		IF_DEBUG("qtaguid: iface_stat: create(%s): entry=%p\n",
+			 ifname, entry);
 		if (ipv4_is_loopback(ipaddr)) {
 			entry->active = false;
-			pr_debug("iface_stat: create(): disable tracking of "
-				 "loopback dev %s\n", ifname);
+			IF_DEBUG("qtaguid: iface_stat: create(%s): "
+				 "disable tracking of loopback dev\n",
+				 ifname);
 		} else {
 			entry->active = true;
-			pr_debug("iface_stat: create(): enable tracking of "
-				 "dev %s with ip=%pI4\n",
+			IF_DEBUG("qtaguid: iface_stat: create(%s): "
+				 "enable tracking. ip=%pI4\n",
 				 ifname, &ipaddr);
 		}
-		return;
+		goto done_unlock_put;
 	} else if (ipv4_is_loopback(ipaddr)) {
-		pr_debug("iface_stat: create(): ignore loopback dev %s"
-			 " ip=%pI4\n", ifname, &ipaddr);
-		return;
+		IF_DEBUG("qtaguid: iface_stat: create(%s): "
+			 "ignore loopback dev. ip=%pI4\n", ifname, &ipaddr);
+		goto done_unlock_put;
 	}
 
-	new_iface = kzalloc(sizeof(*new_iface), GFP_KERNEL);
-	if (new_iface == NULL) {
-		pr_err("iface_stat: create(): failed to alloc iface_stat\n");
+	new_iface = iface_alloc(ifname);
+	IF_DEBUG("qtaguid: iface_stat: create(%s): done "
+		 "entry=%p ip=%pI4\n", ifname, new_iface, &ipaddr);
+
+done_unlock_put:
+	spin_unlock_bh(&iface_stat_list_lock);
+done_put:
+	if (in_dev)
+		in_dev_put(in_dev);
+}
+
+void iface_stat_create_ipv6(const struct net_device *net_dev,
+			    struct inet6_ifaddr *ifa)
+{
+	struct in_device *in_dev;
+	const char *ifname;
+	struct iface_stat *entry;
+	struct iface_stat *new_iface;
+	int addr_type;
+
+	IF_DEBUG("qtaguid: iface_stat: create6(): ifa=%p netdev=%p->name=%s\n",
+		 ifa, net_dev, net_dev ? net_dev->name : "");
+	if (!net_dev) {
+		pr_err("qtaguid: iface_stat: create6(): no net dev!\n");
 		return;
 	}
-	new_iface->ifname = kstrdup(ifname, GFP_KERNEL);
-	if (new_iface->ifname == NULL) {
-		pr_err("iface_stat: create(): failed to alloc ifname\n");
-		kfree(new_iface);
+	ifname = net_dev->name;
+
+	in_dev = in_dev_get(net_dev);
+	if (!in_dev) {
+		pr_err("qtaguid: iface_stat: create6(%s): no inet dev\n",
+		       ifname);
 		return;
 	}
-	spin_lock_init(&new_iface->tag_stat_list_lock);
 
-	new_iface->active = true;
+	IF_DEBUG("qtaguid: iface_stat: create6(%s): in_dev=%p\n",
+		 ifname, in_dev);
 
-	new_iface->tag_stat_tree = RB_ROOT;
-	spin_lock_irqsave(&iface_stat_list_lock, flags);
-	list_add(&new_iface->list, &iface_stat_list);
-	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
+	if (!ifa) {
+		IF_DEBUG("qtaguid: iface_stat: create6(%s): no matching IP\n",
+			 ifname);
+		goto done_put;
+	}
+	addr_type = ipv6_addr_type(&ifa->addr);
 
-	proc_entry = proc_mkdir(ifname, iface_stat_procdir);
-	new_iface->proc_ptr = proc_entry;
+	spin_lock_bh(&iface_stat_list_lock);
+	entry = get_iface_entry(ifname);
+	if (entry != NULL) {
+		IF_DEBUG("qtaguid: iface_stat: create6(%s): entry=%p\n",
+			 ifname, entry);
+		if (addr_type & IPV6_ADDR_LOOPBACK) {
+			entry->active = false;
+			IF_DEBUG("qtaguid: iface_stat: create6(%s): "
+				 "disable tracking of loopback dev\n",
+				 ifname);
+		} else {
+			entry->active = true;
+			IF_DEBUG("qtaguid: iface_stat: create6(%s): "
+				 "enable tracking. ip=%pI6c\n",
+				 ifname, &ifa->addr);
+		}
+		goto done_unlock_put;
+	} else if (addr_type & IPV6_ADDR_LOOPBACK) {
+		IF_DEBUG("qtaguid: iface_stat: create6(%s): "
+			 "ignore loopback dev. ip=%pI6c\n",
+			 ifname, &ifa->addr);
+		goto done_unlock_put;
+	}
 
-	/* TODO: make root access only */
-	create_proc_read_entry("tx_bytes", proc_iface_perms, proc_entry,
-			read_proc_u64, &new_iface->tx_bytes);
-	create_proc_read_entry("rx_bytes", proc_iface_perms, proc_entry,
-			read_proc_u64, &new_iface->rx_bytes);
-	create_proc_read_entry("tx_packets", proc_iface_perms, proc_entry,
-			read_proc_u64, &new_iface->tx_packets);
-	create_proc_read_entry("rx_packets", proc_iface_perms, proc_entry,
-			read_proc_u64, &new_iface->rx_packets);
-	create_proc_read_entry("active", proc_iface_perms, proc_entry,
-			read_proc_bool, &new_iface->active);
+	new_iface = iface_alloc(ifname);
+	IF_DEBUG("qtaguid: iface_stat: create6(%s): done "
+		 "entry=%p ip=%pI6c\n", ifname, new_iface, &ifa->addr);
 
-	pr_debug("iface_stat: create(): done entry=%p dev=%s ip=%pI4\n",
-		 new_iface, ifname, &ipaddr);
+done_unlock_put:
+	spin_unlock_bh(&iface_stat_list_lock);
+done_put:
+	in_dev_put(in_dev);
 }
 
 static struct sock_tag *get_sock_stat_nl(const struct sock *sk)
 {
-	pr_debug("xt_qtaguid: get_sock_stat_nl(sk=%p)\n", sk);
+	MT_DEBUG("qtaguid: get_sock_stat_nl(sk=%p)\n", sk);
 	return sock_tag_tree_search(&sock_tag_tree, sk);
 }
 
 static struct sock_tag *get_sock_stat(const struct sock *sk)
 {
-	unsigned long flags;
 	struct sock_tag *sock_tag_entry;
-	pr_debug("xt_qtaguid: get_sock_stat(sk=%p)\n", sk);
+	MT_DEBUG("qtaguid: get_sock_stat(sk=%p)\n", sk);
 	if (!sk)
 		return NULL;
-	spin_lock_irqsave(&sock_tag_list_lock, flags);
+	spin_lock_bh(&sock_tag_list_lock);
 	sock_tag_entry = get_sock_stat_nl(sk);
-	spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+	spin_unlock_bh(&sock_tag_list_lock);
 	return sock_tag_entry;
 }
 
 static void
-data_counters_update(struct data_counters *dc,  enum ifs_tx_rx direction,
-		int proto, int bytes)
+data_counters_update(struct data_counters *dc, int set,
+		     enum ifs_tx_rx direction, int proto, int bytes)
 {
 	switch (proto) {
 	case IPPROTO_TCP:
-		dc_add_byte_packets(dc, direction, IFS_TCP, bytes, 1);
+		dc_add_byte_packets(dc, set, direction, IFS_TCP, bytes, 1);
 		break;
 	case IPPROTO_UDP:
-		dc_add_byte_packets(dc, direction, IFS_UDP, bytes, 1);
+		dc_add_byte_packets(dc, set, direction, IFS_UDP, bytes, 1);
 		break;
 	case IPPROTO_IP:
 	default:
-		dc_add_byte_packets(dc, direction, IFS_PROTO_OTHER, bytes, 1);
+		dc_add_byte_packets(dc, set, direction, IFS_PROTO_OTHER, bytes,
+				    1);
 		break;
 	}
 }
 
-
 /*
  * Update stats for the specified interface. Do nothing if the entry
  * does not exist (when a device was never configured with an IP address).
  * Called when an device is being unregistered.
  */
-void iface_stat_update(struct net_device *dev)
+static void iface_stat_update(struct net_device *dev)
 {
 	struct rtnl_link_stats64 dev_stats, *stats;
 	struct iface_stat *entry;
-	stats = dev_get_stats(dev, &dev_stats);
-	ASSERT_RTNL();
 
-	entry = get_iface_stat(dev->name);
+	stats = dev_get_stats(dev, &dev_stats);
+	spin_lock_bh(&iface_stat_list_lock);
+	entry = get_iface_entry(dev->name);
 	if (entry == NULL) {
-		pr_debug("iface_stat: dev %s monitor not found\n", dev->name);
+		IF_DEBUG("qtaguid: iface_stat: update(%s): not tracked\n",
+			 dev->name);
+		spin_unlock_bh(&iface_stat_list_lock);
 		return;
 	}
+	IF_DEBUG("qtaguid: iface_stat: update(%s): entry=%p\n",
+		 dev->name, entry);
 	if (entry->active) {
 		entry->tx_bytes += stats->tx_bytes;
 		entry->tx_packets += stats->tx_packets;
 		entry->rx_bytes += stats->rx_bytes;
 		entry->rx_packets += stats->rx_packets;
 		entry->active = false;
-		pr_debug("iface_stat: Updating stats for "
-			"dev %s which went down\n", dev->name);
+		IF_DEBUG("qtaguid: iface_stat: update(%s): "
+			 " disable tracking. rx/tx=%llu/%llu\n",
+			 dev->name, stats->rx_bytes, stats->tx_bytes);
 	} else {
-		pr_debug("iface_stat: Did not update stats for "
-			"dev %s which went down\n", dev->name);
+		IF_DEBUG("qtaguid: iface_stat: update(%s): disabled\n",
+			dev->name);
 	}
+	spin_unlock_bh(&iface_stat_list_lock);
 }
 
-
 static void tag_stat_update(struct tag_stat *tag_entry,
 			enum ifs_tx_rx direction, int proto, int bytes)
 {
-	pr_debug("xt_qtaguid: tag_stat_update(tag=0x%llx (uid=%d) dir=%d "
-		"proto=%d bytes=%d)\n",
-		tag_entry->tag, get_uid_from_tag(tag_entry->tag), direction,
-		proto, bytes);
-	data_counters_update(&tag_entry->counters, direction, proto, bytes);
+	int active_set;
+	active_set = get_active_counter_set(tag_entry->tn.tag);
+	MT_DEBUG("qtaguid: tag_stat_update(tag=0x%llx (uid=%u) set=%d "
+		 "dir=%d proto=%d bytes=%d)\n",
+		 tag_entry->tn.tag, get_uid_from_tag(tag_entry->tn.tag),
+		 active_set, direction, proto, bytes);
+	data_counters_update(&tag_entry->counters, active_set, direction,
+			     proto, bytes);
 	if (tag_entry->parent_counters)
-		data_counters_update(tag_entry->parent_counters, direction,
-				proto, bytes);
+		data_counters_update(tag_entry->parent_counters, active_set,
+				     direction, proto, bytes);
 }
 
-
-/* Create a new entry for tracking the specified {acct_tag,uid_tag} within
+/*
+ * Create a new entry for tracking the specified {acct_tag,uid_tag} within
  * the interface.
- * iface_entry->tag_stat_list_lock should be held. */
+ * iface_entry->tag_stat_list_lock should be held.
+ */
 static struct tag_stat *create_if_tag_stat(struct iface_stat *iface_entry,
 					   tag_t tag)
 {
 	struct tag_stat *new_tag_stat_entry = NULL;
-	pr_debug("iface_stat: create_if_tag_stat(): ife=%p tag=0x%llx"
-		 " (uid=%d)\n",
+	IF_DEBUG("qtaguid: iface_stat: create_if_tag_stat(): ife=%p tag=0x%llx"
+		 " (uid=%u)\n",
 		 iface_entry, tag, get_uid_from_tag(tag));
 	new_tag_stat_entry = kzalloc(sizeof(*new_tag_stat_entry), GFP_ATOMIC);
 	if (!new_tag_stat_entry) {
-		pr_err("iface_stat: failed to alloc new tag entry\n");
+		pr_err("qtaguid: iface_stat: tag stat alloc failed\n");
 		goto done;
 	}
-	new_tag_stat_entry->tag = tag;
+	new_tag_stat_entry->tn.tag = tag;
 	tag_stat_tree_insert(new_tag_stat_entry, &iface_entry->tag_stat_tree);
 done:
 	return new_tag_stat_entry;
 }
 
-static struct iface_stat *get_iface_entry(const char *ifname)
-{
-	struct iface_stat *iface_entry;
-	unsigned long flags;
-
-	/* Find the entry for tracking the specified tag within the interface */
-	if (ifname == NULL) {
-		pr_info("iface_stat: NULL device name\n");
-		return NULL;
-	}
-
-
-	/* Iterate over interfaces */
-	spin_lock_irqsave(&iface_stat_list_lock, flags);
-	list_for_each_entry(iface_entry, &iface_stat_list, list) {
-		if (!strcmp(ifname, iface_entry->ifname))
-			goto done;
-	}
-	iface_entry = NULL;
-done:
-	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
-	return iface_entry;
-}
-
 static void if_tag_stat_update(const char *ifname, uid_t uid,
 			       const struct sock *sk, enum ifs_tx_rx direction,
 			       int proto, int bytes)
@@ -626,25 +867,27 @@ static void if_tag_stat_update(const char *ifname, uid_t uid,
 	struct data_counters *uid_tag_counters;
 	struct sock_tag *sock_tag_entry;
 	struct iface_stat *iface_entry;
-	unsigned long flags;
 	struct tag_stat *new_tag_stat;
-	pr_debug("xt_qtaguid: if_tag_stat_update(ifname=%s "
-		"uid=%d sk=%p dir=%d proto=%d bytes=%d)\n",
+	MT_DEBUG("qtaguid: if_tag_stat_update(ifname=%s "
+		"uid=%u sk=%p dir=%d proto=%d bytes=%d)\n",
 		 ifname, uid, sk, direction, proto, bytes);
 
 
 	iface_entry = get_iface_entry(ifname);
 	if (!iface_entry) {
-		pr_err("iface_stat: interface %s not found\n", ifname);
+		pr_err("qtaguid: iface_stat: stat_update() %s not found\n",
+		       ifname);
 		return;
 	}
-	/* else { If the iface_entry becomes inactive, it is still ok
-	 * to process the data. } */
+	/* It is ok to process data when an iface_entry is inactive */
 
-	pr_debug("iface_stat: stat_update() got entry=%p\n", iface_entry);
+	MT_DEBUG("qtaguid: iface_stat: stat_update() dev=%s entry=%p\n",
+		 ifname, iface_entry);
 
-	/* Look for a tagged sock.
-	 * It will have an acct_uid. */
+	/*
+	 * Look for a tagged sock.
+	 * It will have an acct_uid.
+	 */
 	sock_tag_entry = get_sock_stat(sk);
 	if (sock_tag_entry) {
 		tag = sock_tag_entry->tag;
@@ -655,19 +898,21 @@ static void if_tag_stat_update(const char *ifname, uid_t uid,
 		acct_tag = 0;
 		tag = combine_atag_with_uid(acct_tag, uid);
 	}
-	pr_debug("iface_stat: stat_update(): looking for tag=0x%llx (uid=%d)"
-		 " in ife=%p\n",
+	MT_DEBUG("qtaguid: iface_stat: stat_update(): "
+		 " looking for tag=0x%llx (uid=%u) in ife=%p\n",
 		 tag, get_uid_from_tag(tag), iface_entry);
 	/* Loop over tag list under this interface for {acct_tag,uid_tag} */
-	spin_lock_irqsave(&iface_entry->tag_stat_list_lock, flags);
+	spin_lock_bh(&iface_entry->tag_stat_list_lock);
 
 	tag_stat_entry = tag_stat_tree_search(&iface_entry->tag_stat_tree,
 					      tag);
 	if (tag_stat_entry) {
-		/* Updating the {acct_tag, uid_tag} entry handles both stats:
-		 * {0, uid_tag} will also get updated. */
+		/*
+		 * Updating the {acct_tag, uid_tag} entry handles both stats:
+		 * {0, uid_tag} will also get updated.
+		 */
 		tag_stat_update(tag_stat_entry, direction, proto, bytes);
-		spin_unlock_irqrestore(&iface_entry->tag_stat_list_lock, flags);
+		spin_unlock_bh(&iface_entry->tag_stat_list_lock);
 		return;
 	}
 
@@ -690,7 +935,7 @@ static void if_tag_stat_update(const char *ifname, uid_t uid,
 		new_tag_stat = create_if_tag_stat(iface_entry, tag);
 		new_tag_stat->parent_counters = uid_tag_counters;
 	}
-	spin_unlock_irqrestore(&iface_entry->tag_stat_list_lock, flags);
+	spin_unlock_bh(&iface_entry->tag_stat_list_lock);
 	tag_stat_update(new_tag_stat, direction, proto, bytes);
 }
 
@@ -701,42 +946,76 @@ static int iface_netdev_event_handler(struct notifier_block *nb,
 	if (unlikely(module_passive))
 		return NOTIFY_DONE;
 
-	pr_debug("iface_stat: netdev_event(): ev=0x%lx netdev=%p->name=%s\n",
+	IF_DEBUG("qtaguid: iface_stat: netdev_event(): "
+		 "ev=0x%lx netdev=%p->name=%s\n",
 		 event, dev, dev ? dev->name : "");
 
 	switch (event) {
 	case NETDEV_UP:
-	case NETDEV_REBOOT:
-	case NETDEV_CHANGE:
-	case NETDEV_REGISTER:  /* Most likely no IP */
-	case NETDEV_CHANGEADDR:  /* MAC addr change */
-	case NETDEV_CHANGENAME:
-	case NETDEV_FEAT_CHANGE:  /* Might be usefull when cell type changes */
-		iface_stat_create(dev);
+		iface_stat_create(dev, NULL);
 		break;
-	case NETDEV_UNREGISTER:
+	case NETDEV_DOWN:
 		iface_stat_update(dev);
 		break;
 	}
 	return NOTIFY_DONE;
 }
 
-static int iface_inetaddr_event_handler(struct notifier_block *nb,
-					unsigned long event, void *ptr) {
+static int iface_inet6addr_event_handler(struct notifier_block *nb,
+					 unsigned long event, void *ptr)
+{
+	struct inet6_ifaddr *ifa = ptr;
+	struct net_device *dev;
+
+	if (unlikely(module_passive))
+		return NOTIFY_DONE;
+
+	IF_DEBUG("qtaguid: iface_stat: inet6addr_event(): "
+		 "ev=0x%lx ifa=%p\n",
+		 event, ifa);
+
+	switch (event) {
+	case NETDEV_UP:
+		BUG_ON(!ifa || !ifa->idev);
+		dev = (struct net_device *)ifa->idev->dev;
+		iface_stat_create_ipv6(dev, ifa);
+		atomic64_inc(&qtu_events.iface_events);
+		break;
+	case NETDEV_DOWN:
+		BUG_ON(!ifa || !ifa->idev);
+		dev = (struct net_device *)ifa->idev->dev;
+		iface_stat_update(dev);
+		atomic64_inc(&qtu_events.iface_events);
+		break;
+	}
+	return NOTIFY_DONE;
+}
 
+static int iface_inetaddr_event_handler(struct notifier_block *nb,
+					unsigned long event, void *ptr)
+{
 	struct in_ifaddr *ifa = ptr;
-	struct in_device *in_dev = ifa->ifa_dev;
-	struct net_device *dev = in_dev->dev;
+	struct net_device *dev;
 
 	if (unlikely(module_passive))
 		return NOTIFY_DONE;
 
-	pr_debug("iface_stat: inetaddr_event(): ev=0x%lx netdev=%p->name=%s\n",
-		 event, dev, dev ? dev->name : "");
+	IF_DEBUG("qtaguid: iface_stat: inetaddr_event(): "
+		 "ev=0x%lx ifa=%p\n",
+		 event, ifa);
 
 	switch (event) {
 	case NETDEV_UP:
-		iface_stat_create(dev);
+		BUG_ON(!ifa || !ifa->ifa_dev);
+		dev = ifa->ifa_dev->dev;
+		iface_stat_create(dev, ifa);
+		atomic64_inc(&qtu_events.iface_events);
+		break;
+	case NETDEV_DOWN:
+		BUG_ON(!ifa || !ifa->ifa_dev);
+		dev = ifa->ifa_dev->dev;
+		iface_stat_update(dev);
+		atomic64_inc(&qtu_events.iface_events);
 		break;
 	}
 	return NOTIFY_DONE;
@@ -750,28 +1029,43 @@ static struct notifier_block iface_inetaddr_notifier_blk = {
 	.notifier_call = iface_inetaddr_event_handler,
 };
 
+static struct notifier_block iface_inet6addr_notifier_blk = {
+	.notifier_call = iface_inet6addr_event_handler,
+};
+
 static int __init iface_stat_init(struct proc_dir_entry *parent_procdir)
 {
 	int err;
 
 	iface_stat_procdir = proc_mkdir(iface_stat_procdirname, parent_procdir);
 	if (!iface_stat_procdir) {
-		pr_err("iface_stat: failed to create proc entry\n");
+		pr_err("qtaguid: iface_stat: init failed to create proc entry\n");
 		err = -1;
 		goto err;
 	}
 	err = register_netdevice_notifier(&iface_netdev_notifier_blk);
 	if (err) {
-		pr_err("iface_stat: failed to register dev event handler\n");
-		goto err_unreg_nd;
+		pr_err("qtaguid: iface_stat: init "
+		       "failed to register dev event handler\n");
+		goto err_zap_entry;
 	}
 	err = register_inetaddr_notifier(&iface_inetaddr_notifier_blk);
 	if (err) {
-		pr_err("iface_stat: failed to register dev event handler\n");
-		goto err_zap_entry;
+		pr_err("qtaguid: iface_stat: init "
+		       "failed to register ipv4 dev event handler\n");
+		goto err_unreg_nd;
+	}
+
+	err = register_inet6addr_notifier(&iface_inet6addr_notifier_blk);
+	if (err) {
+		pr_err("qtaguid: iface_stat: init "
+		       "failed to register ipv6 dev event handler\n");
+		goto err_unreg_ip4_addr;
 	}
 	return 0;
 
+err_unreg_ip4_addr:
+	unregister_inetaddr_notifier(&iface_inetaddr_notifier_blk);
 err_unreg_nd:
 	unregister_netdevice_notifier(&iface_netdev_notifier_blk);
 err_zap_entry:
@@ -786,11 +1080,13 @@ static struct sock *qtaguid_find_sk(const struct sk_buff *skb,
 	struct sock *sk;
 	unsigned int hook_mask = (1 << par->hooknum);
 
-	pr_debug("xt_qtaguid: find_sk(skb=%p) hooknum=%d family=%d\n", skb,
+	MT_DEBUG("qtaguid: find_sk(skb=%p) hooknum=%d family=%d\n", skb,
 		 par->hooknum, par->family);
 
-	/* Let's not abuse the the xt_socket_get*_sk(), or else it will
-	 * return garbage SKs. */
+	/*
+	 * Let's not abuse the the xt_socket_get*_sk(), or else it will
+	 * return garbage SKs.
+	 */
 	if (!(hook_mask & XT_SOCKET_SUPPORTED_HOOKS))
 		return NULL;
 
@@ -805,12 +1101,13 @@ static struct sock *qtaguid_find_sk(const struct sk_buff *skb,
 		return NULL;
 	}
 
-	/* Seems to be issues on the file ptr for TCP_TIME_WAIT SKs.
+	/*
+	 * Seems to be issues on the file ptr for TCP_TIME_WAIT SKs.
 	 * http://kerneltrap.org/mailarchive/linux-netdev/2010/10/21/6287959
 	 * Not fixed in 3.0-r3 :(
 	 */
 	if (sk) {
-		pr_debug("xt_qtaguid: %p->sk_proto=%u "
+		MT_DEBUG("qtaguid: %p->sk_proto=%u "
 			 "->sk_state=%d\n", sk, sk->sk_protocol, sk->sk_state);
 		if (sk->sk_state  == TCP_TIME_WAIT) {
 			xt_socket_put_sk(sk);
@@ -827,14 +1124,14 @@ static void account_for_uid(const struct sk_buff *skb,
 	const struct net_device *el_dev;
 
 	if (!skb->dev) {
-		pr_debug("xt_qtaguid[%d]: no skb->dev\n", par->hooknum);
+		MT_DEBUG("qtaguid[%d]: no skb->dev\n", par->hooknum);
 		el_dev = par->in ? : par->out;
 	} else {
 		const struct net_device *other_dev;
 		el_dev = skb->dev;
 		other_dev = par->in ? : par->out;
 		if (el_dev != other_dev) {
-			pr_debug("xt_qtaguid[%d]: skb->dev=%p %s vs "
+			MT_DEBUG("qtaguid[%d]: skb->dev=%p %s vs "
 				"par->(in/out)=%p %s\n",
 				par->hooknum, el_dev, el_dev->name, other_dev,
 				other_dev->name);
@@ -842,14 +1139,14 @@ static void account_for_uid(const struct sk_buff *skb,
 	}
 
 	if (unlikely(!el_dev)) {
-		pr_info("xt_qtaguid[%d]: no par->in/out?!!\n", par->hooknum);
+		pr_info("qtaguid[%d]: no par->in/out?!!\n", par->hooknum);
 	} else if (unlikely(!el_dev->name)) {
-		pr_info("xt_qtaguid[%d]: no dev->name?!!\n", par->hooknum);
+		pr_info("qtaguid[%d]: no dev->name?!!\n", par->hooknum);
 	} else {
-		pr_debug("xt_qtaguid[%d]: dev name=%s type=%d\n",
-			par->hooknum,
-			el_dev->name,
-			el_dev->type);
+		MT_DEBUG("qtaguid[%d]: dev name=%s type=%d\n",
+			 par->hooknum,
+			 el_dev->name,
+			 el_dev->type);
 
 		if_tag_stat_update(el_dev->name, uid,
 				skb->sk ? skb->sk : alternate_sk,
@@ -867,7 +1164,10 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	uid_t sock_uid;
 	bool res;
 
-	pr_debug("xt_qtaguid[%d]: entered skb=%p par->in=%p/out=%p fam=%d\n",
+	if (unlikely(module_passive))
+		return (info->match ^ info->invert) == 0;
+
+	MT_DEBUG("qtaguid[%d]: entered skb=%p par->in=%p/out=%p fam=%d\n",
 		 par->hooknum, skb, par->in, par->out, par->family);
 
 	if (skb == NULL) {
@@ -878,38 +1178,50 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	sk = skb->sk;
 
 	if (sk == NULL) {
-		/*  A missing sk->sk_socket happens when packets are in-flight
+		/*
+		 * A missing sk->sk_socket happens when packets are in-flight
 		 * and the matching socket is already closed and gone.
 		 */
 		sk = qtaguid_find_sk(skb, par);
-		/* If we got the socket from the find_sk(), we will need to put
-		 * it back, as nf_tproxy_get_sock_v4() got it. */
+		/*
+		 * If we got the socket from the find_sk(), we will need to put
+		 * it back, as nf_tproxy_get_sock_v4() got it.
+		 */
 		got_sock = sk;
+		if (sk)
+			atomic64_inc(&qtu_events.match_found_sk_in_ct);
+	} else {
+		atomic64_inc(&qtu_events.match_found_sk);
 	}
-	pr_debug("xt_qtaguid[%d]: sk=%p got_sock=%d proto=%d\n",
+	MT_DEBUG("qtaguid[%d]: sk=%p got_sock=%d proto=%d\n",
 		par->hooknum, sk, got_sock, ip_hdr(skb)->protocol);
 	if (sk != NULL) {
-		pr_debug("xt_qtaguid[%d]: sk=%p->sk_socket=%p->file=%p\n",
+		MT_DEBUG("qtaguid[%d]: sk=%p->sk_socket=%p->file=%p\n",
 			par->hooknum, sk, sk->sk_socket,
 			sk->sk_socket ? sk->sk_socket->file : (void *)-1LL);
 		filp = sk->sk_socket ? sk->sk_socket->file : NULL;
-		pr_debug("xt_qtaguid[%d]: filp...uid=%d\n",
+		MT_DEBUG("qtaguid[%d]: filp...uid=%u\n",
 			par->hooknum, filp ? filp->f_cred->fsuid : -1);
 	}
 
 	if (sk == NULL || sk->sk_socket == NULL) {
-		/* Here, the qtaguid_find_sk() using connection tracking
+		/*
+		 * Here, the qtaguid_find_sk() using connection tracking
 		 * couldn't find the owner, so for now we just count them
-		 * against the system. */
-		/* TODO: unhack how to force just accounting.
+		 * against the system.
+		 */
+		/*
+		 * TODO: unhack how to force just accounting.
 		 * For now we only do iface stats when the uid-owner is not
-		 * requested */
+		 * requested.
+		 */
 		if (!(info->match & XT_QTAGUID_UID))
 			account_for_uid(skb, sk, 0, par);
-		pr_debug("xt_qtaguid[%d]: leaving (sk?sk->sk_socket)=%p\n",
+		MT_DEBUG("qtaguid[%d]: leaving (sk?sk->sk_socket)=%p\n",
 			par->hooknum,
 			sk ? sk->sk_socket : NULL);
 		res = (info->match ^ info->invert) == 0;
+		atomic64_inc(&qtu_events.match_found_sk_none);
 		goto put_sock_ret_res;
 	} else if (info->match & info->invert & XT_QTAGUID_SOCKET) {
 		res = false;
@@ -917,18 +1229,21 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 	}
 	filp = sk->sk_socket->file;
 	if (filp == NULL) {
-		pr_debug("xt_qtaguid[%d]: leaving filp=NULL\n", par->hooknum);
+		MT_DEBUG("qtaguid[%d]: leaving filp=NULL\n", par->hooknum);
 		res = ((info->match ^ info->invert) &
 			(XT_QTAGUID_UID | XT_QTAGUID_GID)) == 0;
 		goto put_sock_ret_res;
 	}
 	sock_uid = filp->f_cred->fsuid;
-	/* TODO: unhack how to force just accounting.
-	 * For now we only do iface stats when the uid-owner is not requested */
+	/*
+	 * TODO: unhack how to force just accounting.
+	 * For now we only do iface stats when the uid-owner is not requested
+	 */
 	if (!(info->match & XT_QTAGUID_UID))
 		account_for_uid(skb, sk, sock_uid, par);
 
-	/* The following two tests fail the match when:
+	/*
+	 * The following two tests fail the match when:
 	 *    id not in range AND no inverted condition requested
 	 * or id     in range AND    inverted condition requested
 	 * Thus (!a && b) || (a && !b) == a ^ b
@@ -937,7 +1252,7 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 		if ((filp->f_cred->fsuid >= info->uid_min &&
 		     filp->f_cred->fsuid <= info->uid_max) ^
 		    !(info->invert & XT_QTAGUID_UID)) {
-			pr_debug("xt_qtaguid[%d]: leaving uid not matching\n",
+			MT_DEBUG("qtaguid[%d]: leaving uid not matching\n",
 				 par->hooknum);
 			res = false;
 			goto put_sock_ret_res;
@@ -946,20 +1261,20 @@ static bool qtaguid_mt(const struct sk_buff *skb, struct xt_action_param *par)
 		if ((filp->f_cred->fsgid >= info->gid_min &&
 				filp->f_cred->fsgid <= info->gid_max) ^
 			!(info->invert & XT_QTAGUID_GID)) {
-			pr_debug("xt_qtaguid[%d]: leaving gid not matching\n",
+			MT_DEBUG("qtaguid[%d]: leaving gid not matching\n",
 				par->hooknum);
 			res = false;
 			goto put_sock_ret_res;
 		}
 
-	pr_debug("xt_qtaguid[%d]: leaving matched\n", par->hooknum);
+	MT_DEBUG("qtaguid[%d]: leaving matched\n", par->hooknum);
 	res = true;
 
 put_sock_ret_res:
 	if (got_sock)
 		xt_socket_put_sk(sk);
 ret_res:
-	pr_debug("xt_qtaguid[%d]: left %d\n", par->hooknum, res);
+	MT_DEBUG("qtaguid[%d]: left %d\n", par->hooknum, res);
 	return res;
 }
 
@@ -973,7 +1288,6 @@ static int qtaguid_ctrl_proc_read(char *page, char **num_items_returned,
 {
 	char *outp = page;
 	int len;
-	unsigned long flags;
 	uid_t uid;
 	struct sock_tag *sock_tag_entry;
 	struct rb_node *node;
@@ -984,29 +1298,59 @@ static int qtaguid_ctrl_proc_read(char *page, char **num_items_returned,
 		return 0;
 	}
 
-	pr_debug("xt_qtaguid: proc ctrl page=%p off=%ld char_count=%d *eof=%d\n",
+	/* TODO: support skipping num_items_returned on entry. */
+	CT_DEBUG("qtaguid: proc ctrl page=%p off=%ld char_count=%d *eof=%d\n",
 		page, items_to_skip, char_count, *eof);
 
 	if (*eof)
 		return 0;
 
-	spin_lock_irqsave(&sock_tag_list_lock, flags);
+	spin_lock_bh(&sock_tag_list_lock);
 	for (node = rb_first(&sock_tag_tree);
 	     node;
 	     node = rb_next(node)) {
 		if (item_index++ < items_to_skip)
 			continue;
-		sock_tag_entry = rb_entry(node, struct sock_tag, node);
+		sock_tag_entry = rb_entry(node, struct sock_tag, sock_node);
 		uid = get_uid_from_tag(sock_tag_entry->tag);
-		pr_debug("xt_qtaguid: proc_read(): sk=%p tag=0x%llx (uid=%d)\n",
-			sock_tag_entry->sk,
-			sock_tag_entry->tag,
-			uid);
+		CT_DEBUG("qtaguid: proc_read(): sk=%p tag=0x%llx (uid=%u)\n",
+			 sock_tag_entry->sk,
+			 sock_tag_entry->tag,
+			 uid
+			);
 		len = snprintf(outp, char_count,
 			       "sock=%p tag=0x%llx (uid=%u)\n",
 			       sock_tag_entry->sk, sock_tag_entry->tag, uid);
 		if (len >= char_count) {
-			spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+			spin_unlock_bh(&sock_tag_list_lock);
+			*outp = '\0';
+			return outp - page;
+		}
+		outp += len;
+		char_count -= len;
+		(*num_items_returned)++;
+	}
+	spin_unlock_bh(&sock_tag_list_lock);
+
+	if (item_index++ >= items_to_skip) {
+		len = snprintf(outp, char_count,
+			       "events: sockets_tagged=%llu "
+			       "sockets_untagged=%llu "
+			       "counter_set_changes=%llu "
+			       "delete_cmds=%llu "
+			       "iface_events=%llu "
+			       "match_found_sk=%llu "
+			       "match_found_sk_in_ct=%llu "
+			       "match_found_sk_none=%llu\n",
+			       atomic64_read(&qtu_events.sockets_tagged),
+			       atomic64_read(&qtu_events.sockets_untagged),
+			       atomic64_read(&qtu_events.counter_set_changes),
+			       atomic64_read(&qtu_events.delete_cmds),
+			       atomic64_read(&qtu_events.iface_events),
+			       atomic64_read(&qtu_events.match_found_sk),
+			       atomic64_read(&qtu_events.match_found_sk_in_ct),
+			       atomic64_read(&qtu_events.match_found_sk_none));
+		if (len >= char_count) {
 			*outp = '\0';
 			return outp - page;
 		}
@@ -1014,124 +1358,215 @@ static int qtaguid_ctrl_proc_read(char *page, char **num_items_returned,
 		char_count -= len;
 		(*num_items_returned)++;
 	}
-	spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+
 	*eof = 1;
 	return outp - page;
 }
 
-int can_impersonate_uid(uid_t uid)
+static bool can_manipulate_uids(void)
 {
-	return uid == current_fsuid()
-		|| !proc_ctrl_write_gid
+	/* root pwnd */
+	return unlikely(!current_fsuid()) || unlikely(!proc_ctrl_write_gid)
 		|| in_egroup_p(proc_ctrl_write_gid);
 }
 
-int can_read_other_uid_stats(uid_t uid)
+static bool can_impersonate_uid(uid_t uid)
 {
-	return uid == current_fsuid()
-		|| !proc_ctrl_write_gid
+	return uid == current_fsuid() || can_manipulate_uids();
+}
+
+static bool can_read_other_uid_stats(uid_t uid)
+{
+	/* root pwnd */
+	return unlikely(!current_fsuid()) || uid == current_fsuid()
+		|| unlikely(!proc_stats_readall_gid)
 		|| in_egroup_p(proc_stats_readall_gid);
 }
 
-/* Delete socket tags, and stat tags associated with a given
- * accouting tag and uid. */
+/*
+ * Delete socket tags, and stat tags associated with a given
+ * accouting tag and uid.
+ */
 static int ctrl_cmd_delete(const char *input)
 {
 	char cmd;
-	uid_t uid = 0;
+	uid_t uid;
 	uid_t entry_uid;
-	tag_t acct_tag = 0;
+	tag_t acct_tag;
 	tag_t tag;
 	int res, argc;
-	unsigned long flags, flags2;
 	struct iface_stat *iface_entry;
 	struct rb_node *node;
 	struct sock_tag *st_entry;
+	struct rb_root st_to_free_tree = RB_ROOT;
 	struct tag_stat *ts_entry;
+	struct tag_counter_set *tcs_entry;
 
-	pr_debug("xt_qtaguid: ctrl_delete(%s): entered\n", input);
 	argc = sscanf(input, "%c %llu %u", &cmd, &acct_tag, &uid);
-	pr_debug("xt_qtaguid: ctrl_delete(%s): argc=%d cmd=%c "
-		 "acct_tag=0x%llx uid=%u\n", input, argc, cmd,
+	CT_DEBUG("qtaguid: ctrl_delete(%s): argc=%d cmd=%c "
+		 "user_tag=0x%llx uid=%u\n", input, argc, cmd,
 		 acct_tag, uid);
 	if (argc < 2) {
 		res = -EINVAL;
 		goto err;
 	}
 	if (!valid_atag(acct_tag)) {
-		pr_info("xt_qtaguid: ctrl_delete(%s): invalid tag\n", input);
+		pr_info("qtaguid: ctrl_delete(%s): invalid tag\n", input);
 		res = -EINVAL;
 		goto err;
 	}
 	if (argc < 3) {
 		uid = current_fsuid();
 	} else if (!can_impersonate_uid(uid)) {
-		pr_info("xt_qtaguid: ctrl_delete(%s): insuficient priv\n",
-			input);
+		pr_info("qtaguid: ctrl_delete(%s): "
+			"insufficient priv from pid=%u uid=%u\n",
+			input, current->pid, current_fsuid());
 		res = -EPERM;
 		goto err;
 	}
 
-	spin_lock_irqsave(&sock_tag_list_lock, flags);
+	/* Delete socket tags */
+	spin_lock_bh(&sock_tag_list_lock);
 	node = rb_first(&sock_tag_tree);
 	while (node) {
-		st_entry = rb_entry(node, struct sock_tag, node);
+		st_entry = rb_entry(node, struct sock_tag, sock_node);
 		entry_uid = get_uid_from_tag(st_entry->tag);
 		node = rb_next(node);
 		if (entry_uid != uid)
 			continue;
 
 		if (!acct_tag || st_entry->tag == tag) {
-			pr_debug("xt_qtaguid: ctrl_delete(): "
-				 "erase sk=%p tag=0x%llx (uid=%d)\n",
-				 st_entry->sk,
-				 st_entry->tag,
-				 entry_uid);
-			rb_erase(&ts_entry->node, &sock_tag_tree);
-			kfree(st_entry);
+			rb_erase(&st_entry->sock_node, &sock_tag_tree);
+			/* Can't sockfd_put() within spinlock, do it later. */
+			sock_tag_tree_insert(st_entry, &st_to_free_tree);
 		}
 	}
-	spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+	spin_unlock_bh(&sock_tag_list_lock);
+
+	node = rb_first(&st_to_free_tree);
+	while (node) {
+		st_entry = rb_entry(node, struct sock_tag, sock_node);
+		node = rb_next(node);
+		CT_DEBUG("qtaguid: ctrl_delete(): "
+			 "erase st: sk=%p tag=0x%llx (uid=%u)\n",
+			 st_entry->sk,
+			 st_entry->tag,
+			 entry_uid);
+		rb_erase(&st_entry->sock_node, &st_to_free_tree);
+		sockfd_put(st_entry->socket);
+		kfree(st_entry);
+	}
 
-	/* If acct_tag is 0, then all entries belonging to uid are
-	 * erased. */
 	tag = combine_atag_with_uid(acct_tag, uid);
-	spin_lock_irqsave(&iface_stat_list_lock, flags);
-	list_for_each_entry(iface_entry, &iface_stat_list, list) {
 
-		spin_lock_irqsave(&iface_entry->tag_stat_list_lock, flags2);
+	/* Delete tag counter-sets */
+	spin_lock_bh(&tag_counter_set_list_lock);
+	tcs_entry = tag_counter_set_tree_search(&tag_counter_set_tree, tag);
+	if (tcs_entry) {
+		CT_DEBUG("qtaguid: ctrl_delete(): "
+			 "erase tcs: tag=0x%llx (uid=%u) set=%d\n",
+			 tcs_entry->tn.tag,
+			 get_uid_from_tag(tcs_entry->tn.tag),
+			 tcs_entry->active_set);
+		rb_erase(&tcs_entry->tn.node, &tag_counter_set_tree);
+		kfree(tcs_entry);
+	}
+	spin_unlock_bh(&tag_counter_set_list_lock);
+
+	/*
+	 * If acct_tag is 0, then all entries belonging to uid are
+	 * erased.
+	 */
+	spin_lock_bh(&iface_stat_list_lock);
+	list_for_each_entry(iface_entry, &iface_stat_list, list) {
+		spin_lock_bh(&iface_entry->tag_stat_list_lock);
 		node = rb_first(&iface_entry->tag_stat_tree);
 		while (node) {
-			ts_entry = rb_entry(node, struct tag_stat, node);
-			entry_uid = get_uid_from_tag(ts_entry->tag);
+			ts_entry = rb_entry(node, struct tag_stat, tn.node);
+			entry_uid = get_uid_from_tag(ts_entry->tn.tag);
 			node = rb_next(node);
 			if (entry_uid != uid)
 				continue;
-			if (!acct_tag || ts_entry->tag == tag) {
-				pr_debug("xt_qtaguid: ctrl_delete(): erase "
-					 "%s 0x%llx %u\n",
+			if (!acct_tag || ts_entry->tn.tag == tag) {
+				CT_DEBUG("qtaguid: ctrl_delete(): "
+					 "erase ts: %s 0x%llx %u\n",
 					 iface_entry->ifname,
-					 get_atag_from_tag(ts_entry->tag),
+					 get_atag_from_tag(ts_entry->tn.tag),
 					 entry_uid);
-				rb_erase(&ts_entry->node,
+				rb_erase(&ts_entry->tn.node,
 					 &iface_entry->tag_stat_tree);
 				kfree(ts_entry);
 			}
 		}
-		spin_unlock_irqrestore(&iface_entry->tag_stat_list_lock,
-				       flags2);
+		spin_unlock_bh(&iface_entry->tag_stat_list_lock);
+	}
+	spin_unlock_bh(&iface_stat_list_lock);
+	atomic64_inc(&qtu_events.delete_cmds);
+	res = 0;
 
+err:
+	return res;
+}
+
+static int ctrl_cmd_counter_set(const char *input)
+{
+	char cmd;
+	uid_t uid = 0;
+	tag_t tag;
+	int res, argc;
+	struct tag_counter_set *tcs;
+	int counter_set;
+
+	argc = sscanf(input, "%c %d %u", &cmd, &counter_set, &uid);
+	CT_DEBUG("qtaguid: ctrl_counterset(%s): argc=%d cmd=%c "
+		 "set=%d uid=%u\n", input, argc, cmd,
+		 counter_set, uid);
+	if (argc != 3) {
+		res = -EINVAL;
+		goto err;
+	}
+	if (counter_set < 0 || counter_set >= IFS_MAX_COUNTER_SETS) {
+		pr_info("qtaguid: ctrl_counterset(%s): invalid counter_set range\n",
+			input);
+		res = -EINVAL;
+		goto err;
+	}
+	if (!can_manipulate_uids()) {
+		pr_info("qtaguid: ctrl_counterset(%s): "
+			"insufficient priv from pid=%u uid=%u\n",
+			input, current->pid, current_fsuid());
+		res = -EPERM;
+		goto err;
 	}
-	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
 
+	tag = make_tag_from_uid(uid);
+	spin_lock_bh(&tag_counter_set_list_lock);
+	tcs = tag_counter_set_tree_search(&tag_counter_set_tree, tag);
+	if (!tcs) {
+		tcs = kzalloc(sizeof(*tcs), GFP_ATOMIC);
+		if (!tcs) {
+			spin_unlock_bh(&tag_counter_set_list_lock);
+			pr_err("qtaguid: ctrl_counterset(%s): "
+			       "failed to alloc counter set\n",
+			       input);
+			res = -ENOMEM;
+			goto err;
+		}
+		tcs->tn.tag = tag;
+		tag_counter_set_tree_insert(tcs, &tag_counter_set_tree);
+		CT_DEBUG("qtaguid: ctrl_counterset(%s): added tcs tag=0x%llx "
+			 "(uid=%u) set=%d\n",
+			 input, tag, get_uid_from_tag(tag), counter_set);
+	}
+	tcs->active_set = counter_set;
+	spin_unlock_bh(&tag_counter_set_list_lock);
+	atomic64_inc(&qtu_events.counter_set_changes);
 	res = 0;
 
 err:
-	pr_debug("xt_qtaguid: ctrl_delete(%s) res=%d\n", input, res);
 	return res;
 }
 
-
 static int ctrl_cmd_tag(const char *input)
 {
 	char cmd;
@@ -1139,113 +1574,157 @@ static int ctrl_cmd_tag(const char *input)
 	uid_t uid = 0;
 	tag_t acct_tag = 0;
 	struct socket *el_socket;
+	int refcnt = -1;
 	int res, argc;
 	struct sock_tag *sock_tag_entry;
-	unsigned long flags;
 
 	/* Unassigned args will get defaulted later. */
 	argc = sscanf(input, "%c %d %llu %u", &cmd, &sock_fd, &acct_tag, &uid);
-	pr_debug("xt_qtaguid: ctrl_tag(%s): argc=%d cmd=%c sock_fd=%d "
+	CT_DEBUG("qtaguid: ctrl_tag(%s): argc=%d cmd=%c sock_fd=%d "
 		 "acct_tag=0x%llx uid=%u\n", input, argc, cmd, sock_fd,
 		 acct_tag, uid);
 	if (argc < 2) {
 		res = -EINVAL;
 		goto err;
 	}
-	el_socket = sockfd_lookup(sock_fd, &res);
+	el_socket = sockfd_lookup(sock_fd, &res);  /* This locks the file */
 	if (!el_socket) {
-		pr_info("xt_qtaguid: ctrl_tag(%s): failed to lookup"
+		pr_info("qtaguid: ctrl_tag(%s): failed to lookup"
 			" sock_fd=%d err=%d\n", input, sock_fd, res);
 		goto err;
 	}
+	refcnt = atomic_read(&el_socket->file->f_count);
+	CT_DEBUG("qtaguid: ctrl_tag(%s): socket->...->f_count=%d\n",
+		 input, refcnt);
 	if (argc < 3) {
 		acct_tag = 0;
 	} else if (!valid_atag(acct_tag)) {
-		pr_info("xt_qtaguid: ctrl_tag(%s): invalid tag\n", input);
+		pr_info("qtaguid: ctrl_tag(%s): invalid tag\n", input);
 		res = -EINVAL;
-		goto err;
+		goto err_put;
 	}
+	CT_DEBUG("qtaguid: ctrl_tag(%s): "
+		 "uid=%u euid=%u fsuid=%u "
+		 "in_group=%d in_egroup=%d\n",
+		 input, current_uid(), current_euid(), current_fsuid(),
+		 in_group_p(proc_stats_readall_gid),
+		 in_egroup_p(proc_stats_readall_gid));
 	if (argc < 4) {
 		uid = current_fsuid();
 	} else if (!can_impersonate_uid(uid)) {
-		pr_info("xt_qtaguid: ctrl_tag(%s): insuficient priv\n",
-			input);
+		pr_info("qtaguid: ctrl_tag(%s): "
+			"insufficient priv from pid=%u uid=%u\n",
+			input, current->pid, current_fsuid());
 		res = -EPERM;
-		goto err;
+		goto err_put;
 	}
 
-	spin_lock_irqsave(&sock_tag_list_lock, flags);
+	spin_lock_bh(&sock_tag_list_lock);
 	sock_tag_entry = get_sock_stat_nl(el_socket->sk);
 	if (sock_tag_entry) {
+		/*
+		 * This is a re-tagging, so release the sock_fd that was
+		 * locked at the time of the 1st tagging.
+		 */
+		sockfd_put(sock_tag_entry->socket);
+		refcnt--;
 		sock_tag_entry->tag = combine_atag_with_uid(acct_tag,
 							    uid);
 	} else {
-		spin_unlock_irqrestore(&sock_tag_list_lock, flags);
 		sock_tag_entry = kzalloc(sizeof(*sock_tag_entry),
-					 GFP_KERNEL);
+					 GFP_ATOMIC);
 		if (!sock_tag_entry) {
+			pr_err("qtaguid: ctrl_tag(%s): "
+			       "socket tag alloc failed\n",
+			       input);
+			spin_unlock_bh(&sock_tag_list_lock);
 			res = -ENOMEM;
-			goto err;
+			goto err_put;
 		}
 		sock_tag_entry->sk = el_socket->sk;
+		sock_tag_entry->socket = el_socket;
 		sock_tag_entry->tag = combine_atag_with_uid(acct_tag,
 							    uid);
-		spin_lock_irqsave(&sock_tag_list_lock, flags);
 		sock_tag_tree_insert(sock_tag_entry, &sock_tag_tree);
+		atomic64_inc(&qtu_events.sockets_tagged);
 	}
-	spin_unlock_irqrestore(&sock_tag_list_lock, flags);
-
-	pr_debug("xt_qtaguid: tag: sock_tag_entry->sk=%p "
-		 "...->tag=0x%llx (uid=%u)\n",
-		 sock_tag_entry->sk, sock_tag_entry->tag,
-		 get_uid_from_tag(sock_tag_entry->tag));
-	res = 0;
+	spin_unlock_bh(&sock_tag_list_lock);
+	/* We keep the ref to the socket (file) until it is untagged */
+	CT_DEBUG("qtaguid: ctrl_tag(%s): done. socket->...->f_count=%d\n",
+		 input,
+		 el_socket ? atomic_read(&el_socket->file->f_count) : -1);
+	return 0;
 
+err_put:
+	/* Release the sock_fd that was grabbed by sockfd_lookup(). */
+	sockfd_put(el_socket);
+	refcnt--;
 err:
-	pr_debug("xt_qtaguid: ctrl_tag(%s) res=%d\n", input, res);
+	CT_DEBUG("qtaguid: ctrl_tag(%s): done. socket->...->f_count=%d\n",
+		 input, refcnt);
 	return res;
 }
 
-
 static int ctrl_cmd_untag(const char *input)
 {
 	char cmd;
 	int sock_fd = 0;
 	struct socket *el_socket;
+	int refcnt = -1;
 	int res, argc;
 	struct sock_tag *sock_tag_entry;
-	unsigned long flags;
 
-	pr_debug("xt_qtaguid: ctrl_untag(%s): entered\n", input);
 	argc = sscanf(input, "%c %d", &cmd, &sock_fd);
-	pr_debug("xt_qtaguid: ctrl_untag(%s): argc=%d cmd=%c sock_fd=%d\n",
+	CT_DEBUG("qtaguid: ctrl_untag(%s): argc=%d cmd=%c sock_fd=%d\n",
 		 input, argc, cmd, sock_fd);
 	if (argc < 2) {
 		res = -EINVAL;
 		goto err;
 	}
-	el_socket = sockfd_lookup(sock_fd, &res);
+	el_socket = sockfd_lookup(sock_fd, &res);  /* This locks the file */
 	if (!el_socket) {
-		pr_info("xt_qtaguid: ctrl_untag(%s): failed to lookup"
+		pr_info("qtaguid: ctrl_untag(%s): failed to lookup"
 			" sock_fd=%d err=%d\n", input, sock_fd, res);
 		goto err;
 	}
-	spin_lock_irqsave(&sock_tag_list_lock, flags);
+	refcnt = atomic_read(&el_socket->file->f_count);
+	CT_DEBUG("qtaguid: ctrl_untag(%s): socket->...->f_count=%d\n",
+		 input, refcnt);
+	spin_lock_bh(&sock_tag_list_lock);
 	sock_tag_entry = get_sock_stat_nl(el_socket->sk);
 	if (!sock_tag_entry) {
-		spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+		spin_unlock_bh(&sock_tag_list_lock);
 		res = -EINVAL;
-		goto err;
+		goto err_put;
 	}
-	/* The socket already belongs to the current process
-	 * so it can do whatever it wants to it. */
-	rb_erase(&sock_tag_entry->node, &sock_tag_tree);
-	spin_unlock_irqrestore(&sock_tag_list_lock, flags);
+	/*
+	 * The socket already belongs to the current process
+	 * so it can do whatever it wants to it.
+	 */
+	rb_erase(&sock_tag_entry->sock_node, &sock_tag_tree);
+
+	/*
+	 * Release the sock_fd that was grabbed at tag time,
+	 * and once more for the sockfd_lookup() here.
+	 */
+	sockfd_put(sock_tag_entry->socket);
+	spin_unlock_bh(&sock_tag_list_lock);
+	sockfd_put(el_socket);
+	refcnt -= 2;
 	kfree(sock_tag_entry);
+	atomic64_inc(&qtu_events.sockets_untagged);
+	CT_DEBUG("qtaguid: ctrl_untag(%s): done. socket->...->f_count=%d\n",
+		 input, refcnt);
 
-	res = 0;
+	return 0;
+
+err_put:
+	/* Release the sock_fd that was grabbed by sockfd_lookup(). */
+	sockfd_put(el_socket);
+	refcnt--;
 err:
-	pr_debug("xt_qtaguid: ctrl_untag(%s): res=%d\n", input, res);
+	CT_DEBUG("qtaguid: ctrl_untag(%s): done. socket->...->f_count=%d\n",
+		 input, refcnt);
 	return res;
 }
 
@@ -1254,7 +1733,6 @@ static int qtaguid_ctrl_parse(const char *input, int count)
 	char cmd;
 	int res;
 
-	pr_debug("xt_qtaguid: ctrl(%s): entered\n", input);
 	cmd = input[0];
 	/* Collect params for commands */
 	switch (cmd) {
@@ -1262,6 +1740,10 @@ static int qtaguid_ctrl_parse(const char *input, int count)
 		res = ctrl_cmd_delete(input);
 		break;
 
+	case 's':
+		res = ctrl_cmd_counter_set(input);
+		break;
+
 	case 't':
 		res = ctrl_cmd_tag(input);
 		break;
@@ -1277,7 +1759,7 @@ static int qtaguid_ctrl_parse(const char *input, int count)
 	if (!res)
 		res = count;
 err:
-	pr_debug("xt_qtaguid: ctrl(%s): res=%d\n", input, res);
+	CT_DEBUG("qtaguid: ctrl(%s): res=%d\n", input, res);
 	return res;
 }
 
@@ -1300,14 +1782,22 @@ static int qtaguid_ctrl_proc_write(struct file *file, const char __user *buffer,
 	return qtaguid_ctrl_parse(input_buf, count);
 }
 
-static int print_stats_line(char *outp, int char_count, int item_index,
-			    char *ifname, tag_t tag,
-			    struct data_counters *counters)
+struct proc_print_info {
+	char *outp;
+	char **num_items_returned;
+	struct iface_stat *iface_entry;
+	struct tag_stat *ts_entry;
+	int item_index;
+	int char_count;
+};
+
+static int pp_stats_line(struct proc_print_info *ppi, int cnt_set)
 {
 	int len;
-	if (!item_index) {
-		len = snprintf(outp, char_count,
-			       "idx iface acct_tag_hex uid_tag_int "
+	struct data_counters *cnts;
+	if (!ppi->item_index) {
+		len = snprintf(ppi->outp, ppi->char_count,
+			       "idx iface acct_tag_hex uid_tag_int cnt_set "
 			       "rx_bytes rx_packets "
 			       "tx_bytes tx_packets "
 			       "rx_tcp_packets rx_tcp_bytes "
@@ -1317,47 +1807,73 @@ static int print_stats_line(char *outp, int char_count, int item_index,
 			       "tx_udp_packets tx_udp_bytes "
 			       "tx_other_packets tx_other_bytes\n");
 	} else {
+		tag_t tag = ppi->ts_entry->tn.tag;
 		uid_t stat_uid = get_uid_from_tag(tag);
 		if (!can_read_other_uid_stats(stat_uid)) {
-			pr_debug("xt_qtaguid: insufficient priv for stat line:"
-				 "%s 0x%llx %u\n",
-				 ifname, get_atag_from_tag(tag), stat_uid);
+			CT_DEBUG("qtaguid: stats line: "
+				 "%s 0x%llx %u: "
+				 "insufficient priv from pid=%u uid=%u\n",
+				 ppi->iface_entry->ifname,
+				 get_atag_from_tag(tag), stat_uid,
+				 current->pid, current_fsuid());
 			return 0;
 		}
-		len = snprintf(outp, char_count,
-			       "%d %s 0x%llx %u "
-			       "%llu %llu "
-			       "%llu %llu "
-			       "%llu %llu "
-			       "%llu %llu "
-			       "%llu %llu "
-			       "%llu %llu "
-			       "%llu %llu "
-			       "%llu %llu\n",
-			       item_index,
-			       ifname,
-			       get_atag_from_tag(tag),
-			       stat_uid,
-			       dc_sum_bytes(counters, IFS_RX),
-			       dc_sum_packets(counters, IFS_RX),
-			       dc_sum_bytes(counters, IFS_TX),
-			       dc_sum_packets(counters, IFS_TX),
-			       counters->bpc[IFS_RX][IFS_TCP].bytes,
-			       counters->bpc[IFS_RX][IFS_TCP].packets,
-			       counters->bpc[IFS_RX][IFS_UDP].bytes,
-			       counters->bpc[IFS_RX][IFS_UDP].packets,
-			       counters->bpc[IFS_RX][IFS_PROTO_OTHER].bytes,
-			       counters->bpc[IFS_RX][IFS_PROTO_OTHER].packets,
-			       counters->bpc[IFS_TX][IFS_TCP].bytes,
-			       counters->bpc[IFS_TX][IFS_TCP].packets,
-			       counters->bpc[IFS_TX][IFS_UDP].bytes,
-			       counters->bpc[IFS_TX][IFS_UDP].packets,
-			       counters->bpc[IFS_TX][IFS_PROTO_OTHER].bytes,
-			       counters->bpc[IFS_TX][IFS_PROTO_OTHER].packets);
+		cnts = &ppi->ts_entry->counters;
+		len = snprintf(
+			ppi->outp, ppi->char_count,
+			"%d %s 0x%llx %u %u "
+			"%llu %llu "
+			"%llu %llu "
+			"%llu %llu "
+			"%llu %llu "
+			"%llu %llu "
+			"%llu %llu "
+			"%llu %llu "
+			"%llu %llu\n",
+			ppi->item_index,
+			ppi->iface_entry->ifname,
+			get_atag_from_tag(tag),
+			stat_uid,
+			cnt_set,
+			dc_sum_bytes(cnts, cnt_set, IFS_RX),
+			dc_sum_packets(cnts, cnt_set, IFS_RX),
+			dc_sum_bytes(cnts, cnt_set, IFS_TX),
+			dc_sum_packets(cnts, cnt_set, IFS_TX),
+			cnts->bpc[cnt_set][IFS_RX][IFS_TCP].bytes,
+			cnts->bpc[cnt_set][IFS_RX][IFS_TCP].packets,
+			cnts->bpc[cnt_set][IFS_RX][IFS_UDP].bytes,
+			cnts->bpc[cnt_set][IFS_RX][IFS_UDP].packets,
+			cnts->bpc[cnt_set][IFS_RX][IFS_PROTO_OTHER].bytes,
+			cnts->bpc[cnt_set][IFS_RX][IFS_PROTO_OTHER].packets,
+			cnts->bpc[cnt_set][IFS_TX][IFS_TCP].bytes,
+			cnts->bpc[cnt_set][IFS_TX][IFS_TCP].packets,
+			cnts->bpc[cnt_set][IFS_TX][IFS_UDP].bytes,
+			cnts->bpc[cnt_set][IFS_TX][IFS_UDP].packets,
+			cnts->bpc[cnt_set][IFS_TX][IFS_PROTO_OTHER].bytes,
+			cnts->bpc[cnt_set][IFS_TX][IFS_PROTO_OTHER].packets);
 	}
 	return len;
 }
 
+bool pp_sets(struct proc_print_info *ppi)
+{
+	int len;
+	int counter_set;
+	for (counter_set = 0; counter_set < IFS_MAX_COUNTER_SETS;
+	     counter_set++) {
+		len = pp_stats_line(ppi, counter_set);
+		if (len >= ppi->char_count) {
+			*ppi->outp = '\0';
+			return false;
+		}
+		if (len) {
+			ppi->outp += len;
+			ppi->char_count -= len;
+			(*ppi->num_items_returned)++;
+		}
+	}
+	return true;
+}
 
 /*
  * Procfs reader to get all tag stats using style "1)" as described in
@@ -1368,19 +1884,23 @@ static int qtaguid_stats_proc_read(char *page, char **num_items_returned,
 				off_t items_to_skip, int char_count, int *eof,
 				void *data)
 {
-	char *outp = page;
+	struct proc_print_info ppi;
 	int len;
-	unsigned long flags, flags2;
-	struct iface_stat *iface_entry;
-	struct tag_stat *ts_entry;
-	int item_index = 0;
+
+	ppi.outp = page;
+	ppi.item_index = 0;
+	ppi.char_count = char_count;
+	ppi.num_items_returned = num_items_returned;
 
 	if (unlikely(module_passive)) {
+		len = pp_stats_line(&ppi, 0);
+		/* The header should always be shorter than the buffer. */
+		WARN_ON(len >= ppi.char_count);
 		*eof = 1;
-		return 0;
+		return len;
 	}
 
-	pr_debug("xt_qtaguid:proc stats page=%p *num_items_returned=%p off=%ld "
+	CT_DEBUG("qtaguid:proc stats page=%p *num_items_returned=%p off=%ld "
 		"char_count=%d *eof=%d\n", page, *num_items_returned,
 		items_to_skip, char_count, *eof);
 
@@ -1389,53 +1909,39 @@ static int qtaguid_stats_proc_read(char *page, char **num_items_returned,
 
 	if (!items_to_skip) {
 		/* The idx is there to help debug when things go belly up. */
-		len = print_stats_line(outp, char_count, /*index*/0, NULL,
-				       make_tag_from_uid(0), NULL);
+		len = pp_stats_line(&ppi, 0);
 		/* Don't advance the outp unless the whole line was printed */
-		if (len >= char_count) {
-			*outp = '\0';
-			return outp - page;
+		if (len >= ppi.char_count) {
+			*ppi.outp = '\0';
+			return ppi.outp - page;
 		}
-		outp += len;
-		char_count -= len;
+		ppi.outp += len;
+		ppi.char_count -= len;
 	}
-	spin_lock_irqsave(&iface_stat_list_lock, flags);
-	list_for_each_entry(iface_entry, &iface_stat_list, list) {
+
+	spin_lock_bh(&iface_stat_list_lock);
+	list_for_each_entry(ppi.iface_entry, &iface_stat_list, list) {
 		struct rb_node *node;
-		spin_lock_irqsave(&iface_entry->tag_stat_list_lock, flags2);
-		for (node = rb_first(&iface_entry->tag_stat_tree);
+		spin_lock_bh(&ppi.iface_entry->tag_stat_list_lock);
+		for (node = rb_first(&ppi.iface_entry->tag_stat_tree);
 		     node;
 		     node = rb_next(node)) {
-			ts_entry = rb_entry(node, struct tag_stat, node);
-			if (item_index++ < items_to_skip)
+			ppi.ts_entry = rb_entry(node, struct tag_stat, tn.node);
+			if (ppi.item_index++ < items_to_skip)
 				continue;
-			len = print_stats_line(outp, char_count,
-					       item_index,
-					       iface_entry->ifname,
-					       ts_entry->tag,
-					       &ts_entry->counters);
-			if (len >= char_count) {
-				*outp = '\0';
-				spin_unlock_irqrestore(
-					&iface_entry->tag_stat_list_lock,
-					flags2);
-				spin_unlock_irqrestore(
-					&iface_stat_list_lock, flags);
-				return outp - page;
-			}
-			if (len) {
-				outp += len;
-				char_count -= len;
-				(*num_items_returned)++;
+			if (!pp_sets(&ppi)) {
+				spin_unlock_bh(
+					&ppi.iface_entry->tag_stat_list_lock);
+				spin_unlock_bh(&iface_stat_list_lock);
+				return ppi.outp - page;
 			}
 		}
-		spin_unlock_irqrestore(&iface_entry->tag_stat_list_lock,
-				flags2);
+		spin_unlock_bh(&ppi.iface_entry->tag_stat_list_lock);
 	}
-	spin_unlock_irqrestore(&iface_stat_list_lock, flags);
+	spin_unlock_bh(&iface_stat_list_lock);
 
 	*eof = 1;
-	return outp - page;
+	return ppi.outp - page;
 }
 
 /*------------------------------------------*/
@@ -1444,7 +1950,7 @@ static int __init qtaguid_proc_register(struct proc_dir_entry **res_procdir)
 	int ret;
 	*res_procdir = proc_mkdir(module_procdirname, init_net.proc_net);
 	if (!*res_procdir) {
-		pr_err("xt_qtaguid: failed to create proc/.../xt_qtaguid\n");
+		pr_err("qtaguid: failed to create proc/.../xt_qtaguid\n");
 		ret = -ENOMEM;
 		goto no_dir;
 	}
@@ -1452,7 +1958,7 @@ static int __init qtaguid_proc_register(struct proc_dir_entry **res_procdir)
 	xt_qtaguid_ctrl_file = create_proc_entry("ctrl", proc_ctrl_perms,
 						*res_procdir);
 	if (!xt_qtaguid_ctrl_file) {
-		pr_err("xt_qtaguid: failed to create xt_qtaguid/ctrl "
+		pr_err("qtaguid: failed to create xt_qtaguid/ctrl "
 			" file\n");
 		ret = -ENOMEM;
 		goto no_ctrl_entry;
@@ -1463,7 +1969,7 @@ static int __init qtaguid_proc_register(struct proc_dir_entry **res_procdir)
 	xt_qtaguid_stats_file = create_proc_entry("stats", proc_stats_perms,
 						*res_procdir);
 	if (!xt_qtaguid_stats_file) {
-		pr_err("xt_qtaguid: failed to create xt_qtaguid/stats "
+		pr_err("qtaguid: failed to create xt_qtaguid/stats "
 			"file\n");
 		ret = -ENOMEM;
 		goto no_stats_entry;
@@ -1505,7 +2011,8 @@ static int __init qtaguid_mt_init(void)
 	return 0;
 }
 
-/* TODO: allow unloading of the module.
+/*
+ * TODO: allow unloading of the module.
  * For now stats are permanent.
  * Kconfig forces'y/n' and never an 'm'.
  */